Commit 88b46f77 authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

Update SysFilter usage and tests

parent 47cb1799
Pipeline #514 passed with stage
in 1 minute and 26 seconds
......@@ -66,18 +66,21 @@ end
Often one finds the need to implement a stateful controller, i.e., a function
that has a memory or state. To this end, the function [`sysfilter`](@ref) is
provided. This function is used to implement control loops where a signal is
filtered through a dynamical system, i.e., `U(z) = C(z)E(z)`.
that has a memory or state. To this end, the type [`SysFilter`](@ref) is
provided. This type is used to implement control loops where a signal is
filtered through a dynamical system, i.e., `U(z) = G1(z)E(z)`.
Usage is demonstrated below, which is a simplified implementation of the block
diagram above (transfer function- and signal names corresponds to the figure).
First two `SysFilter` objects are created, these objects can now be used as
functions of an input, and return the filtered output. The `SysFilter` type takes
care of updating remembering the state of the system when called.
```julia
stateG1 = init_sysfilter(G1)
stateG4 = init_sysfilter(G4)
G1f = SysFilter(G1)
G4f = SysFilter(G4)
function control(i)
rf = sysfilter!(stateG4, G4, r)
rf = G4f(r)
e = rf-y
u = sysfilter!(stateG1, G1, e)
u = G1f(e)
end
```
`G1` and `G4` must here be represented by [`StateSpace`](http://juliacontrol.github.io/ControlSystems.jl/latest/lib/constructors/#ControlSystems.ss) types from [`ControlSystems.jl`](https://github.com/JuliaControl/ControlSystems.jl).
......@@ -93,19 +96,18 @@ This is very easy, just get a discrete time `StateSpace` model of your process
You now have to implement the methods `control` and `measure` for your simulated type.
The implementation for `BeamSimulator` is shown below
```julia
function control(p::BeamSimulator, u)
sysfilter!(p.state, p.sys, u)
end
measure(P) = vecdot(p.sys.C,p.state)
control(p::BeamSimulator, u) = p.Gf(u)
measure(P) = vecdot(p.Gf.sys.C, p.Gf.state)
```
The `control` method accepts a control signal (`u`) and propagates the system state
(`p.state`) forward using the statespace model (`p.sys`) of the beam. The function
[`sysfilter!`](@ref) is familiar from the "Control" section above. What it does
is essentially
(`p.Gf.state`) forward using the statespace model (`p.Gf.sys`) of the beam. The object
[`Gf::SysFilter`](@ref) is familiar from the "Control" section above. What it does
is essentially (simplified)
```julia
function sysfilter!(state, sys, input)
state .= sys.A*state + sys.B*input
output = sys.C*state + sys.D*input
function Gf(input)
sys = Gf.sys
Gf.state .= sys.A*Gf.state + sys.B*input
output = sys.C*Gf.state + sys.D*input
end
```
hence, it just performs one iteration of
......@@ -125,19 +127,20 @@ It should now be obvious which fields are required in the `BeamSimulator` type.
It must know which sample time it has been discretized with, as well as its
discrete-time system model. It must also remember the current state of the system.
This is not needed in a physical process since it kind of remembers its own state.
The system model and its state is conveniently covered by the type [`SysFilter`](@ref),
which handles filtering of a signal through an LTI system.
The full type specification for `BeamSimulator` is given below
```julia
struct BeamSimulator <: SimulatedProcess
h::Float64
state::Vector{Float64} # states defined by the file define_beam_system
sys::StateSpace
BeamSimulator() = new(0.01, init_sysfilter(beam_system), c2d(beam_system, 0.01)[1])
BeamSimulator(h::Real) = new(Float64(h), init_sysfilter(beam_system), c2d(beam_system, h)[1])
Gf::SysFilter
BeamSimulator() = new(0.01, SysFilter(beam_system, 0.01))
BeamSimulator(h::Real) = new(Float64(h), SysFilter(beam_system, h))
end
```
It contains three fields and two inner constructors. The constructors initializes
the system state by calling `init_sysfilter`. The variable `beam_system` is already
defined outside the type specification.
the system filter by creating a [`SysFilter`](@ref).
The variable `beam_system` is already defined outside the type specification.
One of the constructors provides a default value for the sample time, in case
the user is unsure about a reasonable value.
......
......@@ -4,11 +4,12 @@ module LabProcesses
using ControlSystems
include("utilities.jl")
include("interface.jl")
include("interface_documentation.jl")
include("interface_implementations/ballandbeam.jl")
include("utilities.jl")
include("reference_generators.jl")
include("controllers.jl")
......
......@@ -18,15 +18,15 @@ function run_control_2DOF(P::AbstractProcess,sysFB, sysFF=nothing; duration = 10
u = zeros(nu, length(0:h:duration))
r = zeros(ny, length(0:h:duration))
stateFB = init_sysfilter(sysFB)
Gfb = SysFilter(sysFB)
if sysFF != nothing
stateFF = init_sysfilter(sysFF)
Gff = SysFilter(sysFF)
end
function calc_control(i)
rf = sysFF == nothing ? r[:,i] : sysfilter!(stateFF, sysFF, r[:,i])
rf = sysFF == nothing ? r[:,i] : Gff(r[:,i])
e = rf-y[:,i]
ui = sysfilter!(stateFB, sysFB, e)
ui = Gfb(e)
ui + bias(P)
end
......
......@@ -22,10 +22,9 @@ const beam_system, nice_beam_controller = define_beam_system()
# nice_beam_controller gives ϕₘ=56°, Aₘ=4, Mₛ = 1.6. Don't forget to discretize it before use
struct BeamSimulator <: SimulatedProcess
h::Float64
state::Vector{Float64} # states defined by the file define_beam_system
sys::StateSpace
BeamSimulator() = new(0.01, init_sysfilter(beam_system), c2d(beam_system, 0.01)[1])
BeamSimulator(h::Real) = new(Float64(h), init_sysfilter(beam_system), c2d(beam_system, h)[1])
s::SysFilter
BeamSimulator() = new(0.01, SysFilter(beam_system, 0.01))
BeamSimulator(h::Real) = new(Float64(h), SysFilter(beam_system, h))
end
struct BallAndBeam <: PhysicalProcess
......@@ -36,9 +35,9 @@ BallAndBeam() = BallAndBeam(0.01, 0.0)
struct BallAndBeamSimulator <: SimulatedProcess
h::Float64
state::Vector{Float64} # pos,vel,angle,ω
s::SysFilter
end
BallAndBeamSimulator() = BallAndBeamSimulator(0.01, zeros(4))
const AbstractBeam = Union{Beam, BeamSimulator}
......@@ -64,9 +63,8 @@ bias(p::BallAndBeamSimulator) = 0
control(p::AbstractBeamOrBallAndBeam, u) = ccall((:comedi_write, comedipath),Int32,
(Int32,Int32,Int32,Int32),0,1,1,num2io(u[1]+p.bias))
function control(p::BeamSimulator, u)
sysfilter!(p.state, p.sys, u)
end
control(p::BeamSimulator, u) = p.s(u)
control(p::BallAndBeamSimulator, u) = error("Not yet implemented")
measure(p::Beam) = io2num(ccall((:comedi_read,comedipath), Int32,
......@@ -74,7 +72,7 @@ measure(p::Beam) = io2num(ccall((:comedi_read,comedipath), Int32,
measure(p::BallAndBeam) = [io2num(ccall((:comedi_read,comedipath), Int32,
(Int32,Int32,Int32), 0,0,i)) for i = 0:1]
measure(p::BeamSimulator) = vecdot(p.sys.C,p.state)
measure(p::BeamSimulator) = vecdot(p.s.sys.C,p.s.state)
measure(p::BallAndBeamSimulator) = error("Not yet implemented")
......@@ -87,5 +85,5 @@ initialize(p::AbstractBeamOrBallAndBeam) = ccall((:comedi_start, comedipath),Int
finalize(p::AbstractBeamOrBallAndBeam) = (control(p,0);ccall((:comedi_stop, comedipath),Int32,(Int32,), 0))
initialize(p::BallAndBeamSimulator) = nothing
finalize(p::BallAndBeamSimulator) = nothing
initialize(p::BeamSimulator) = p.state .*= 0
initialize(p::BeamSimulator) = p.s.state .*= 0
finalize(p::BeamSimulator) = nothing
export @periodically, init_sysfilter, sysfilter!
export @periodically, init_sysfilter, sysfilter!, SysFilter
"""
@periodically(h, body)
......@@ -27,6 +27,36 @@ macro periodically(h, simulation, body)
end
end
"""
Csf = SysFilter(sys_discrete::StateSpace)
Csf = SysFilter(sys_continuous::StateSpace, sampletime)
Csf = SysFilter(sys::StateSpace, state::AbstractVector)
Returns an object used for filtering signals through LTI systems.
Create a SysFilter object that can be used to implement control loops and simulators
with LTI systems, i.e., `U(z) = C(z)E(z)`. To filter a signal `u` through the filter,
call like `y = Csf(u)`. Calculates the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`
"""
struct SysFilter
sys::StateSpace
state::Vector{Float64}
function SysFilter(sys::StateSpace, state::AbstractVector)
@assert !ControlSystems.iscontinuous(sys) "Can not filter using continuous time model."
@assert length(state) == sys.nx "length(state) != sys.nx"
new(sys, state)
end
function SysFilter(sys::StateSpace)
@assert !ControlSystems.iscontinuous(sys) "Can not filter using continuous time model. Supply sample time."
new(sys, init_sysfilter(sys))
end
function SysFilter(sys::StateSpace, h::Real)
@assert ControlSystems.iscontinuous(sys) "Sample time supplied byt system model is already in discrete time."
sysd = c2d(sys, h)[1]
new(sysd, init_sysfilter(sysd))
end
end
(s::SysFilter)(input) = sysfilter!(s.state, s.sys, input)
"""
state = init_sysfilter(sys::StateSpace)
Use together with [`sysfilter!`](@ref)
......@@ -36,6 +66,7 @@ function init_sysfilter(sys::StateSpace)
end
"""
output = sysfilter!(s::SysFilter, input)
output = sysfilter!(state, sys::StateSpace, input)
Returns the filtered output `y` in `y = Cx+Du, x'=Ax+Bu`
......@@ -46,3 +77,5 @@ function sysfilter!(state::AbstractVector, sys::StateSpace, input)
state .= vec(sys.A*state + sys.B*input)
output = vec(sys.C*state + sys.D*input)
end
sysfilter!(s::SysFilter, input) = sysfilter!(s.state, s.sys, input)
using LabProcesses
using LabProcesses, ControlSystems
using Base.Test
# Reference generators
......@@ -6,3 +6,30 @@ r = PRBSGenerator(Int(4))
seq = [r() for i = 1:10]
@test all(seq .== [1,0,1,0,0,0,0,0,0,0])
foreach(r,1:10_000)
function test_sysfilter()
N = 10
u = randn(N)
b = [1, 1]
a = [1, 0.1, 1]
sys = ss(tf(b,a,1))
state = init_sysfilter(sys)
yf = filt(b,a,u)
yff = similar(yf)
for i in eachindex(u)
yff[i] = sysfilter!(state, sys, u[i])[1]
end
@test sum(abs,yf - yff) < (eps())
sysfilt = SysFilter(sys)
for i in eachindex(u)
yff[i] = sysfilter!(sysfilt, u[i])[1]
end
@test sum(abs,yf - yff) < (eps())
sysfilt = SysFilter(sys)
for i in eachindex(u)
yff[i] = sysfilt(u[i])[1]
end
@test sum(abs,yf - yff) < (eps())
end
test_sysfilter()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment