diff --git a/src/BallAndBeam.jl b/src/BallAndBeam.jl
index ab87053654472b0bd9eafe1d50d205e140ba8905..7c7da9c41e9fdd5e469c7287d7be21d5bce5527f 100644
--- a/src/BallAndBeam.jl
+++ b/src/BallAndBeam.jl
@@ -134,12 +134,13 @@ nypl(G; kwargs...) = (plot();nypl!(G;kwargs...))
"""
- sysFB,L,T,C = fbdesign(G, polevect, zerovect, gain)
+ sysFB,L,T,C = fbdesign(G::AbstractMatrix, polevect, zerovect, gain)
Frequency Compensation to determine polynominals in compensator C=S/R.
Frequency responses for loop transfer, closed loop and controller are calculated.
`sys` is of type `StateSpace`, to be used together with [`sysfilter!`](@ref)
+`G` is a frequency response matrix with ω in the first column and G(iω) in the second.
"""
-function fbdesign(G, polevect, zerovect, gain)
+function fbdesign(G::AbstractMatrix, polevect, zerovect, gain)
ω = Float64.(G[:,1])
pzv = isempty(zerovect) ? 1 : prod(abs.(zerovect))
sysFB = ss(zpk(zerovect,polevect,gain*prod(abs.(polevect))/pzv))
diff --git a/src/FRTN35_lab1.jl b/src/FRTN35_lab1.jl
index 16c073f75f41ab6d67800f680b8fdb0c177722fc..7db7989e9b65bb821884ef1c6c46c2453c6fe0c9 100644
--- a/src/FRTN35_lab1.jl
+++ b/src/FRTN35_lab1.jl
@@ -12,7 +12,7 @@ nbr_of_periods = 3
# and run three experiments. You may modify the freqency vectors
# any way you want and add/remove experiments as needed.
-w1_100 = logspace(log10(1),log10(300),8)
+w1_100 = logspace(log10(1),log10(300),800)
G1 = fra(P, w1_100, amplitude=1, nbr_of_periods=nbr_of_periods, settling_time=settling_time)
@save "workspace.jld"
@@ -31,6 +31,8 @@ bopl(G123, m=:star)
nypl(G123, m=:star)
## Control ==================================================================================
+# Specify poles, zeros and gain for your feedback controller and feedforward compensator
+# The input `G` to `fbdesign` is the data you have measured, e.g, `G123`
polevect = [-10]
zerovect = []
gain = 1
@@ -49,3 +51,26 @@ bopl!(YR, lab="Closed-loop system r->y")
sysFB,sysFF = c2d(sysFBc,h)[1],c2d(sysFFc,h)[1]
y,u,r = run_control_2DOF(P, sysFB, sysFF, duration=5, reference = t->2sign(sin(2π/3*t)))
plot([y u r], lab = ["y" "u" "r"])
+
+
+# If you have time, estimate ARX model
+prbs = PRBSGenerator()
+duration = 5
+y = zeros(0:h:duration)
+u = zeros(0:h:duration)
+LabProcesses.initialize(P)
+for (i,t) = enumerate(0:h:duration)
+ y[i] = measure(P)
+ u[i] = prbs()-0.5 + 0.5*u[max(1,i-1)]
+ control(P, u[i])
+end
+LabProcesses.finalize(P)
+
+na = 6
+nb = 5
+arxtf = arx(h, y[:], u, na, nb; λ = 0)
+
+bodeplot([P.sys, arxtf], logspace(-1,3,200))
+
+plot(result.error[1:end-1])
+plot([u y])
diff --git a/src/arx.jl b/src/arx.jl
new file mode 100644
index 0000000000000000000000000000000000000000..1d0569e0a5fbf32487150f2a66908625718c30c3
--- /dev/null
+++ b/src/arx.jl
@@ -0,0 +1,127 @@
+abstract type Model end
+abstract type LinearModel <: Model end
+abstract type NonLinearModel <: Model end
+abstract type Network <: NonLinearModel end
+const Polynom{T<:Real} = Union{Array{T,1} , T}
+const PolynomMatrix{T} = Union{Array{Polynom{T},1},Polynom{T}, T}
+
+
+
+mutable struct FitStatistics
+ RMS::Float64
+ FIT::Float64
+ AIC::Float64
+ FitStatistics(y::Vector{Float64},yh::Vector{Float64},d) = new(rms(y-yh),fit(y,yh),aic(y-yh,d))
+end
+
+mutable struct FitResult
+ error::VecOrMat{Float64}
+ fit::FitStatistics
+ method::Symbol
+ λ::Float64
+ function FitResult(y,yh,d::Int,method::Symbol, λ = 0)
+ error = y-yh
+ new(error, FitStatistics(y,yh, d),method, λ)
+ end
+end
+
+mutable struct IdData
+ y::VecOrMat{Float64}
+ u::VecOrMat{Float64}
+ Ts::Float64
+ IdData(y::VecOrMat{Float64}) = new(y,[],0)
+ IdData(y::VecOrMat{Float64} ,Ts::Float64) = new(y,[],Ts)
+ IdData(y::VecOrMat{Float64}, u::VecOrMat{Float64}) = new(y,u,0)
+end
+
+## Helper functions
+rms(x) = sqrt(mean(x.^2))
+sse(x) = sum(x.^2)
+fit(y,yh) = 100 * (1-rms(y-yh)./rms(y-mean(y)));
+aic(x,d) = log(sse(x)) + 2d/length(x)
+
+##
+Base.show(fit::FitStatistics) = println("Fit RMS:$(fit.RMS), FIT:$(fit.FIT), AIC:$(fit.AIC)")
+
+function toeplitz{T}(c::AbstractVector{T},r::AbstractVector{T})
+ nc = length(c)
+ nr = length(r)
+ A = zeros(T, nc, nr)
+ A[:,1] = c
+ A[1,:] = r
+ for i in 2:nr
+ A[2:end,i] = A[1:end-1,i-1]
+ end
+ A
+end
+
+"""
+`a::Vector{Float64}`: The polynomial coeffs A(z) (not including the first 1)\n
+`b::VecOrMat{Float64}`: The polynomial coeffs B(z)\n
+`na::Int`\n
+`nb::Vector{Int}`\n
+"""
+type ARX <: LinearModel
+ a::Polynom
+ b::PolynomMatrix
+ na::Int
+ nb::Polynom{Int}
+end
+
+
+function getARXregressor(y::AbstractVector{Float64},u::AbstractVecOrMat{Float64}, na, nb)
+ assert(length(nb) == size(u,2))
+ m = max(na+1,maximum(nb))
+ n = length(y) - m+1
+ offs = m-na-1
+ A = toeplitz(y[offs+na+1:n+na+offs],y[offs+na+1:-1:1])
+ y = copy(A[:,1])
+ A = A[:,2:end]
+ for i = 1:length(nb)
+ offs = m-nb[i]
+ A = [A toeplitz(u[nb[i]+offs:n+nb[i]+offs-1,i],u[nb[i]+offs:-1:1+offs,i])]
+ end
+ return y,A
+end
+
+getARXregressor(iddata::IdData, na, nb) = getARXregressor(iddata.y,iddata.u, na, nb)
+
+"""
+ find_na(y::Vector,n::Int)
+Plots the RMSE and AIC for model orders up to `n`. Useful for model selection
+"""
+function find_na(y::Vector,n::Int)
+ error = zeros(n,2)
+ for i = 1:n
+ w,e = ar(y,i)
+ error[i,1] = rms(e)
+ error[i,2] = aic(e,i)
+ print(i,", ")
+ end
+ println("Done")
+ scatter(error, show=true)
+end
+
+"""
+ Gtf = arx(h,y, u, na, nb; λ = 0)
+Fit a transfer function to data using an ARX model.
+`nb` and `na` are the orders of the numerator and denominator polynomials.
+"""
+function arx(h,y::AbstractVector{Float64}, u::AbstractVector{Float64}, na, nb; λ = 0)
+ na -= 1
+ y_train, A = getARXregressor(y,u, na, nb)
+ n_points = size(A,1)
+
+ if λ == 0
+ w = A\y_train
+ method = :LS
+ else
+ w = (A'A + λ*eye(size(A,2)))\A'y_train
+ method = :LS_reg
+ end
+
+ a = [1; -w[1:na]]
+ b = w[na+1:end]
+ model = tf(b,a,h)
+ return model
+end
diff --git a/test/runtests.jl b/test/runtests.jl
index b4f2b660f65094ca00ea4518a7c2dbb6c4b69f49..ccd72776ab913fc6e4f28a588cc660168588281c 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -52,7 +52,6 @@ sysFB,sysFF = c2d(sysFBc,h),c2d(sysFFc,h)
# Test BeamSimulator
-bias = 0.01 # Change this if your process drifts over time
P = LabProcesses.BeamSimulator(0.001) # Replace for BeamSimulator to run simulations
h = sampletime(P)
@@ -68,3 +67,34 @@ phase_id = angle.(G1[:,2]) |> ControlSystems.unwrap
phase_true = angle.(true_resp[1][:]) |> ControlSystems.unwrap
@test sum(abs, (phase_id-phase_true)[1:400]) < 0.065
@test sum(abs, (phase_id-phase_true)[400:end]) < 5.3 # Some numerical errors for high freqs
+
+
+P = LabProcesses.BeamSimulator(0.01)
+h = sampletime(P)
+prbs = PRBSGenerator()
+duration = 5
+y = zeros(0:h:duration)
+u = zeros(0:h:duration)
+LabProcesses.initialize(P)
+for (i,t) = enumerate(0:h:duration)
+ y[i] = measure(P)
+ u[i] = prbs()-0.5 + 0.5*u[max(1,i-1)]
+ control(P, u[i])
+end
+LabProcesses.finalize(P)
+na = 6
+nb = 5
+arxtf = arx(h, y[:], u, na, nb; λ = 0)
+
+w1_100 = logspace(-1,log10(100),500)
+true_resp = freqresp(P.sys, w1_100)
+phase_true = angle.(true_resp[1][:]) |> ControlSystems.unwrap
+arx_resp = freqresp(arxtf, w1_100)
+@test sum(abs, log.(abs.(arx_resp[1][:])) - log.(abs.(true_resp[1][:]))) < 1.25
+
+phase_id = angle.(arx_resp[1][:]) |> ControlSystems.unwrap
+@test sum(abs, (phase_id-phase_true)[1:400]) < 21
+
+
+# plot([log.(abs.(arx_resp[1][:])) log.(abs.(true_resp[1][:]))])
+# bodeplot([P.sys, arxtf])