Add alpha_beta and alpha_beta_gamma trackers

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,

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)

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