diff --git a/lib/ControlSystemsBase/src/ControlSystemsBase.jl b/lib/ControlSystemsBase/src/ControlSystemsBase.jl index bfe4f123a..f9ac95132 100644 --- a/lib/ControlSystemsBase/src/ControlSystemsBase.jl +++ b/lib/ControlSystemsBase/src/ControlSystemsBase.jl @@ -49,6 +49,8 @@ export LTISystem, innovation_form, observer_predictor, observer_filter, + alpha_beta, + alpha_beta_gamma, observer_controller, # Stability Analysis isstable, diff --git a/lib/ControlSystemsBase/src/synthesis.jl b/lib/ControlSystemsBase/src/synthesis.jl index 696da7f29..345eb0461 100644 --- a/lib/ControlSystemsBase/src/synthesis.jl +++ b/lib/ControlSystemsBase/src/synthesis.jl @@ -144,6 +144,157 @@ end @deprecate kalman(A::AbstractMatrix, args...; kwargs...) kalman(Continuous, A, args...; kwargs...) @deprecate dkalman(args...; kwargs...) kalman(Discrete, args...; kwargs...) +""" + alpha_beta(alpha, Ts; beta = 2(2 - alpha) - 4√(1 - alpha)) + +Make an α-β tracker. The tracker estimates the position and the rate of a target. The function +returns a discrete-time `StateSpace` system. The input of the system is the measured position. +The outputs are the estimates ``[x̂, v̂]``. + +```math +\\begin{aligned} +r(k) &= y(k) - \\big(x̂(k-1) + T_s v̂(k-1)\\big) \\\\ +x̂(k) &= x̂(k-1) + T_s v̂(k-1) + α\\, r(k) \\\\ +v̂(k) &= v̂(k-1) + \\dfrac{β}{T_s} r(k) +\\end{aligned} +``` + +The system is [`observer_filter`](@ref) of a double integrator with the gain +``K = [α,\\ β/T_s]``. The state of the system is the a-posteriori estimate. Thus the system is +strictly proper. The rate output is a filtered derivative of the input. A separate differentiator +is not necessary. + +!!! note "The sample index" + The state is ``x̂(k|k)``. This estimate includes the measurement ``y(k)``. The state-space + update is ``x(k+1) = Ax(k) + Bu(k)``. Therefore, in a simulation, this estimate is at output + index ``k+1``. The output at index ``k`` includes the measurements up to ``y(k-1)``. + [`observer_filter`](@ref) uses the same convention. + +`alpha` sets the effect of each measurement on the position estimate. The value of `alpha` must +be in the range ``0 < α < 1``. The value of `beta` must be in the range ``0 < β ≤ 2 - α``. The +default `beta` is Kalata's steady-state relation ``β = 2(2 - α) - 4\\sqrt{1 - α}``. This relation +makes the filter equal to the steady-state Kalman filter for a target that has a constant rate. +Thus `alpha` is a sufficient tuning parameter. + +For a target that accelerates, use [`alpha_beta_gamma`](@ref). If you know the noise covariances +and not the gains, use [`kalman`](@ref) with [`observer_filter`](@ref). + +# Example +The rate estimate converges to the slope of a ramp input. +```jldoctest +julia> using ControlSystemsBase + +julia> Ts = 0.1; sys = alpha_beta(0.5, Ts); + +julia> size(sys) +(2, 1) + +julia> res = lsim(sys, (x, t) -> [t], 0:Ts:5); + +julia> round(res.y[2, end], digits = 3) +1.0 +``` + +# Extended help +The estimation-error dynamics are ``(I - KC)A``. The gains set the eigenvalues of this matrix. +The default `beta` gives a complex pole pair for each value of `alpha`. To put both error poles +on the real axis at the same radius ``s ∈ (0, 1)``, give both gains: + +``α = 1 - s^2``, ``β = (1 - s)^2``. + +These gains make the filter critically damped. The error then decays with no oscillation. + +Do not give only `alpha`. If you give only `alpha`, `beta` keeps its default value, and the poles +do not stay on the real axis. + +``s`` replaces `alpha` as the tuning parameter. The error decays as ``k s^k``. Use +``-T_s/\\ln s`` as an estimate of the time constant. A smaller value of ``s`` gives faster +tracking and more measurement noise in the estimates. +""" +function alpha_beta(alpha, Ts; beta = 2 * (2 - alpha) - 4 * sqrt(1 - alpha)) + 0 < alpha < 1 || throw(ArgumentError("alpha must satisfy 0 < alpha < 1, got $alpha")) + Ts > 0 || throw(ArgumentError("Ts must be positive, got $Ts")) + T = float(promote_type(typeof(alpha), typeof(beta), typeof(Ts))) + A = T[1 Ts; 0 1] + C = T[1 0] + K = T[alpha; beta/Ts;;] + ss((I - K * C) * A, K, Matrix{T}(I, 2, 2), zeros(T, 2, 1), Ts) +end + +""" + alpha_beta_gamma(alpha, Ts; beta = 2(2 - alpha) - 4√(1 - alpha), gamma = beta^2 / (2alpha)) + +Make an α-β-γ tracker. The tracker estimates the position, the rate and the acceleration of a +target. The function returns a discrete-time `StateSpace` system. The input of the system is the +measured position. The outputs are the estimates ``[x̂, v̂, â]``. + +```math +\\begin{aligned} +r(k) &= y(k) - \\big(x̂(k-1) + T_s v̂(k-1) + \\tfrac{T_s^2}{2} â(k-1)\\big) \\\\ +x̂(k) &= x̂(k-1) + T_s v̂(k-1) + \\tfrac{T_s^2}{2} â(k-1) + α\\, r(k) \\\\ +v̂(k) &= v̂(k-1) + T_s â(k-1) + \\dfrac{β}{T_s} r(k) \\\\ +â(k) &= â(k-1) + \\dfrac{γ}{T_s^2} r(k) +\\end{aligned} +``` + +The system is [`observer_filter`](@ref) of a triple integrator with the gain +``K = [α,\\ β/T_s,\\ γ/T_s^2]``. The rule for the sample index that applies to +[`alpha_beta`](@ref) also applies to this filter. + +An α-β tracker predicts with a constant rate. If the target accelerates, the rate estimate of an +α-β tracker has an error. This error is proportional to the acceleration. No value of `alpha` and +`beta` removes this error. This filter also predicts with the acceleration. Thus the error is not +present. + +Use this filter when the sample rate is much higher than the frequency content of the signal. In +this condition, a long filter memory causes only a small lag. The filter then decreases the +effect of the measurement noise. + +The default gains are Kalata's steady-state relations. These gains make the filter equal to the +steady-state Kalman filter for a target that has a constant acceleration. Thus `alpha` is a +sufficient tuning parameter. If ``α = 0.5``, then ``β ≈ 0.1716`` and ``γ ≈ 0.0294``. + +# Example +```jldoctest +julia> using ControlSystemsBase + +julia> sys = alpha_beta_gamma(0.5, 0.1); + +julia> size(sys) +(3, 1) +``` + +# Extended help +The default gains are not critically damped. This is also true for [`alpha_beta`](@ref). The +Kalata gains give a complex pole pair for each value of `alpha`. To put all three error poles on +the real axis at the same radius ``s ∈ (0, 1)``, give all three gains: + +``α = 1 - s^3``, ``β = \\tfrac{3}{2}(1 - s)^2(1 + s)``, ``γ = (1 - s)^3``. + +Do not give only `alpha`. If you give only `alpha`, `beta` and `gamma` keep their default values, +and the poles do not stay on the real axis. For example, at ``s = 0.9`` the critically damped +gains are ``(0.271, 0.0285, 0.001)``. If you give `alpha = 0.271` and no other gain, you get +``β = 0.0427`` and ``γ = 0.0034``. + +The pole has a multiplicity of three. Thus the error decays as ``k^2 s^k``, and the settling time +is longer than the pole radius alone indicates. Use ``-T_s/\\ln s`` as an estimate of the time +constant. At ``s = 0.9`` this estimate is approximately 9.5 samples. + +These relations apply to the gain convention that is shown above. In this convention, the +acceleration correction is ``γ/T_s^2`` and the rate correction is ``β/T_s``. Other references put +``2γ`` in this position. The relations in those references are thus different by a factor of two. +""" +function alpha_beta_gamma(alpha, Ts; beta = 2 * (2 - alpha) - 4 * sqrt(1 - alpha), + gamma = beta^2 / (2 * alpha)) + 0 < alpha < 1 || throw(ArgumentError("alpha must satisfy 0 < alpha < 1, got $alpha")) + Ts > 0 || throw(ArgumentError("Ts must be positive, got $Ts")) + T = float(promote_type(typeof(alpha), typeof(beta), typeof(gamma), typeof(Ts))) + A = T[1 Ts Ts^2/2; 0 1 Ts; 0 0 1] + C = T[1 0 0] + K = T[alpha; beta/Ts; gamma/Ts^2;;] + ss((I - K * C) * A, K, Matrix{T}(I, 3, 3), zeros(T, 3, 1), Ts) +end + """ place(A, B, p, opt=:c; direct = false) place(sys::StateSpace, p, opt=:c; direct = false) diff --git a/lib/ControlSystemsBase/test/test_synthesis.jl b/lib/ControlSystemsBase/test/test_synthesis.jl index 4f321a564..a0a872c3c 100644 --- a/lib/ControlSystemsBase/test/test_synthesis.jl +++ b/lib/ControlSystemsBase/test/test_synthesis.jl @@ -242,3 +242,121 @@ end end end + +@testset "alpha_beta / alpha_beta_gamma" begin + Ts = 0.1 + kalata(a) = 2 * (2 - a) - 4 * sqrt(1 - a) + + @testset "shape and defaults" begin + s2 = alpha_beta(0.5, Ts) + s3 = alpha_beta_gamma(0.5, Ts) + @test size(s2) == (2, 1) + @test size(s3) == (3, 1) + @test s2.Ts == Ts && s3.Ts == Ts + @test iszero(s2.D) && iszero(s3.D) # the state is the estimate + @test s2.C == I && s3.C == I + + # The documented default gains. + @test kalata(0.5) ≈ 0.1715728752538097 + @test kalata(0.5)^2 / (2 * 0.5) ≈ 0.029437251522859908 + # ... and they are what the systems are actually built with: B == K. + @test s2.B ≈ [0.5; kalata(0.5) / Ts;;] + @test s3.B ≈ [0.5; kalata(0.5) / Ts; kalata(0.5)^2 / (2 * 0.5) / Ts^2;;] + end + + @testset "reproduces the documented recurrence" begin + a, b, g = 0.4, 0.2, 0.05 + t = 0:Ts:2 + us = collect(float.(t)) # a unit-slope ramp + + x = 0.0; v = 0.0; X = Float64[]; V = Float64[] + for u in us + pred = x + Ts * v + r = u - pred + x = pred + a * r + v = v + (b / Ts) * r + push!(X, x); push!(V, v) + end + y = lsim(alpha_beta(a, Ts; beta = b), reshape(us, 1, :), t).y + # The estimate that absorbed us[k] appears at output index k+1. + @test y[1, 2:end] ≈ X[1:end-1] + @test y[2, 2:end] ≈ V[1:end-1] + + x = 0.0; v = 0.0; ac = 0.0; X = Float64[]; V = Float64[]; A = Float64[] + for u in us + pred = x + Ts * v + Ts^2 / 2 * ac + predv = v + Ts * ac + r = u - pred + x = pred + a * r + v = predv + (b / Ts) * r + ac = ac + (g / Ts^2) * r + push!(X, x); push!(V, v); push!(A, ac) + end + y = lsim(alpha_beta_gamma(a, Ts; beta = b, gamma = g), reshape(us, 1, :), t).y + @test y[1, 2:end] ≈ X[1:end-1] + @test y[2, 2:end] ≈ V[1:end-1] + @test y[3, 2:end] ≈ A[1:end-1] + end + + @testset "tracks a ramp" begin + t = 0:Ts:5 + res = lsim(alpha_beta(0.5, Ts), (x, t) -> [t], t) + @test res.y[2, end] ≈ 1 atol = 1e-6 # rate converges to the slope + res = lsim(alpha_beta_gamma(0.5, Ts), (x, t) -> [t], t) + @test res.y[2, end] ≈ 1 atol = 1e-3 # third order settles slower on a ramp + @test res.y[3, end] ≈ 0 atol = 1e-3 # and the acceleration to zero + end + + @testset "equals observer_filter of the integrator chain" begin + for (n, f) in ((2, alpha_beta), (3, alpha_beta_gamma)) + A = n == 2 ? [1 Ts; 0 1.0] : [1 Ts Ts^2/2; 0 1 Ts; 0 0 1.0] + B = n == 2 ? [Ts^2/2; Ts] : [Ts^3/6; Ts^2/2; Ts] + C = n == 2 ? [1.0 0] : [1.0 0 0] + sys = ss(A, B, C, 0, Ts) + a, b, g = 0.4, 0.2, 0.05 + K = n == 2 ? [a; b/Ts;;] : [a; b/Ts; g/Ts^2;;] + filt = n == 2 ? f(a, Ts; beta = b) : f(a, Ts; beta = b, gamma = g) + ref = observer_filter(sys, K; output_state = true) + @test filt.A ≈ ref.A + @test filt.B ≈ ref.B[:, 2:2] # observer_filter also takes u; this has only y + end + end + + @testset "critical damping" begin + # The recipes in the extended help place every error pole on the real axis at s. The + # eigenvalue is defective, so it is only conditioned to about sqrt(eps). + for s in (0.95, 0.9, 0.8, 0.5, 0.2) + p = eigvals(alpha_beta(1 - s^2, Ts; beta = (1 - s)^2).A) + @test all(z -> isapprox(z, s; atol = 1e-6), p) + + p = eigvals(alpha_beta_gamma(1 - s^3, Ts; + beta = 1.5 * (1 - s)^2 * (1 + s), gamma = (1 - s)^3).A) + @test all(z -> isapprox(z, s; atol = 1e-4), p) + end + + # Independent of the sample rate: these gains are dimensionless in this parameterization. + for Ts2 in (1.0, 0.01, 1e-4) + p = eigvals(alpha_beta(1 - 0.9^2, Ts2; beta = (1 - 0.9)^2).A) + @test all(z -> isapprox(z, 0.9; atol = 1e-6), p) + end + + # The defaults are a different tuning and always leave a complex pair, which is why the + # recipes have to set every gain rather than just `alpha`. + for a in (0.2, 0.5, 0.9) + @test any(z -> abs(imag(z)) > 1e-6, eigvals(alpha_beta(a, Ts).A)) + @test any(z -> abs(imag(z)) > 1e-6, eigvals(alpha_beta_gamma(a, Ts).A)) + end + # The numbers the docstring quotes for s = 0.9. + @test all((1 - 0.9^3, 1.5 * 0.1^2 * 1.9, 0.1^3) .≈ (0.271, 0.0285, 0.001)) + @test kalata(0.271) ≈ 0.0427 atol = 1e-4 + @test kalata(0.271)^2 / (2 * 0.271) ≈ 0.0034 atol = 1e-4 + end + + @testset "argument checking" begin + @test_throws ArgumentError alpha_beta(0.0, Ts) + @test_throws ArgumentError alpha_beta(1.0, Ts) + @test_throws ArgumentError alpha_beta(0.5, 0.0) + @test_throws ArgumentError alpha_beta_gamma(-0.1, Ts) + @test_throws ArgumentError alpha_beta_gamma(0.5, -1.0) + end +end