From cf29a410ae7c2e9e472beb5ad6dacc6c04483412 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 16 May 2025 12:19:21 +0200 Subject: [PATCH 1/2] optionally return additional arguments from `lqr, kalman` such as the solution to the Riccati equation and the closed-loop poles --- lib/ControlSystemsBase/src/analysis.jl | 9 +++- lib/ControlSystemsBase/src/plotting.jl | 2 - lib/ControlSystemsBase/src/synthesis.jl | 52 +++++++++++++------ lib/ControlSystemsBase/test/test_synthesis.jl | 2 + 4 files changed, 44 insertions(+), 21 deletions(-) diff --git a/lib/ControlSystemsBase/src/analysis.jl b/lib/ControlSystemsBase/src/analysis.jl index b8d20fe93..e5409f085 100644 --- a/lib/ControlSystemsBase/src/analysis.jl +++ b/lib/ControlSystemsBase/src/analysis.jl @@ -282,10 +282,15 @@ function tzeros(A::AbstractMatrix{T}, B::AbstractMatrix{T}, C::AbstractMatrix{T} end (z, iz, KRInfo) = MatrixPencils.spzeros(A, I, B, C, D; kwargs...) + if z isa Vector{<:Real} + zc = complex(z) + else + zc = z + end if E - return (z, iz, KRInfo) + return (zc, iz, KRInfo) else - return filter(isfinite, z) + return filter(isfinite, zc) end end diff --git a/lib/ControlSystemsBase/src/plotting.jl b/lib/ControlSystemsBase/src/plotting.jl index e20657526..d45c8490a 100644 --- a/lib/ControlSystemsBase/src/plotting.jl +++ b/lib/ControlSystemsBase/src/plotting.jl @@ -361,13 +361,11 @@ _span(vec) = -(reverse(extrema(vec))...) label --> "" group --> group_ind phasedata = unwrap ? ControlSystemsBase.unwrap(phasedata.*(pi/180)).*(180/pi) : phasedata - # NOTE: we should only downsample if the user hasn't provided w themselves if adaptive downsample(ws, phasedata, _span(phasedata)/300)[1:2] else ws, phasedata end - end end end diff --git a/lib/ControlSystemsBase/src/synthesis.jl b/lib/ControlSystemsBase/src/synthesis.jl index 490241b5a..e6b620b42 100644 --- a/lib/ControlSystemsBase/src/synthesis.jl +++ b/lib/ControlSystemsBase/src/synthesis.jl @@ -1,7 +1,7 @@ """ - lqr(sys, Q, R) - lqr(Continuous, A, B, Q, R, args...; kwargs...) - lqr(Discrete, A, B, Q, R, args...; kwargs...) + lqr(sys, Q, R; extra = Val(false)) + lqr(Continuous, A, B, Q, R, args...; extra = Val(false), kwargs...) + lqr(Discrete, A, B, Q, R, args...; extra = Val(false), kwargs...) Calculate the optimal gain matrix `K` for the state-feedback law `u = -K*x` that minimizes the cost function: @@ -18,6 +18,8 @@ To obtain also the solution to the Riccati equation and the eigenvalues of the c To obtain a discrete-time approximation to a continuous-time LQR problem, the function [`c2d`](@ref) can be used to obtain corresponding discrete-time cost matrices. +If `extra = Val(true)`, the function returns `K, P, p`, where `P` is the solution to the Riccati equation (Lyapunov function `x'P*x`), and `p` are the eigenvalues of `A-BK`. + # Examples Continuous time ```julia @@ -64,14 +66,14 @@ This function requires - `R` must be positive definite - The pair `(Q,A)` must not have any unobservable modes on the imaginary axis (cont) / unit circle (disc), e.g., there must not be any integrating modes that are not penalized by `Q`. if this condition does not hold, you may get the error "The Hamiltonian matrix is not dichotomic". """ -function lqr(::ContinuousType, A, B, Q, R, args...; kwargs...) - S, _, K = arec(A, B, R, Q, args...; kwargs...) - return K +function lqr(::ContinuousType, A, B, Q, R, args...; extra::Val{E} = Val(false), kwargs...) where E + S, p, K, args... = arec(A, B, R, Q, args...; kwargs...) + E ? (K, S, p, args...) : K end -function lqr(::DiscreteType, A, B, Q, R, args...; kwargs...) - S, _, K = ared(A, B, R, Q, args...; kwargs...) - return K +function lqr(::DiscreteType, A, B, Q, R, args...; extra::Val{E} = Val(false), kwargs...) where E + S, p, K, args... = ared(A, B, R, Q, args...; kwargs...) + E ? (K, S, p, args...) : K end @deprecate lqr(A::AbstractMatrix, args...; kwargs...) lqr(Continuous, A, args...; kwargs...) @@ -80,9 +82,9 @@ end """ - kalman(Continuous, A, C, R1, R2) - kalman(Discrete, A, C, R1, R2; direct = false) - kalman(sys, R1, R2; direct = false) + kalman(Continuous, A, C, R1, R2; extra=Val(false)) + kalman(Discrete, A, C, R1, R2; extra=Val(false), direct = false) + kalman(sys, R1, R2; extra=Val(false), direct = false) Calculate the optimal asymptotic Kalman gain for the linear-Gaussian model ```math @@ -93,26 +95,42 @@ y &= Cx + v ``` where `w` is the dynamics noise with covariance `R1` and `v` is the measurement noise with covariance `R2`. -If `direct = true`, the observer gain is computed for the pair `(A, CA)` instead of `(A,C)`. This option is intended to be used together with the option `direct = true` to [`observer_controller`](@ref). Ref: "Computer-Controlled Systems" pp 140. `direct = false` is sometimes referred to as a "delayed" estimator, while `direct = true` is a "current" estimator. +In discrete time, the returned Kalman gain `K` is designed to be used with (`direct = false`) +```math +x(t+1|t) = (A-KC)x(t|t-1) + Bu(t) + Ky(t) +``` +and (`direct = true`) +```math +x(t+1|t+1) = (A-KC)x(t|t) + Bu(t) + Ky(t+1) +``` + +If `direct = true`, the observer gain is computed as ``K_d = R_∞C^T (R_2 + CR_∞ C^T)^{-1}`` instead of ``K = A K_d``. This option is intended to be used together with the option `direct = true` to [`observer_controller`](@ref). Ref: "Computer-Controlled Systems" pp 140. `direct = false` is sometimes referred to as a "delayed" estimator, while `direct = true` is a "current" estimator. To obtain a discrete-time approximation to a continuous-time LQG problem, the function [`c2d`](@ref) can be used to obtain corresponding discrete-time covariance matrices. To obtain an LTISystem that represents the Kalman filter, pass the obtained Kalman feedback gain into [`observer_filter`](@ref). To obtain an LQG controller, pass the obtained Kalman feedback gain as well as a state-feedback gain computed using [`lqr`](@ref) into [`observer_controller`](@ref). -The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?MatrixEquations.arec/ared` for more help. +If `extra = Val(true)`, the function returns `K, R∞, p`, where `R∞` is the solution to the Riccati equation (the stationary prediction-error covariance matrix, the covariance of ``x̃(t|t-1)``), and `p` are the eigenvalues of `A-KC`. In this case, the _filtering error_ covariance is given by ``(I-KC) R∞``, i.e., the covariance of ``x̃(t|t)``. + +The `args...; kwargs...` are sent to the Riccati solver, allowing specification of cross-covariance etc. See `?ControlSystemsBase.MatrixEquations.arec/ared` for more help. # FAQ This function requires - `R1` must be positive semi-definite - `R2` must be positive definite - The pair `(A,R1)` must not have any uncontrollable modes on the imaginary axis (cont) / unit circle (disc), e.g., there must not be any integrating modes that are not affected through `R1`. if this condition does not hold, you may get the error "The Hamiltonian matrix is not dichotomic". + +# Extended help +`MatrixEquations.ared` solves the Riccati equation corresponding to the direct form, but returns the ``K`` matrix for the indirect form. The solution to the Riccati equation is the stationary prediction-error covariance matrix ``R_∞``, and the filtering error covariance is given by ``(I-A^{-1}KC) R_∞``. If using the direct form, the filter-error covariance is given by ``(I-K_d C) R_∞`` """ -function kalman(te, A, C, R1,R2, args...; direct = false, kwargs...) +function kalman(te, A, C, R1,R2, args...; direct = false, extra::Val{E} = Val(false), kwargs...) where E + Kt, p, R∞, args... = lqr(te, A',C',R1,R2, args...; extra=Val(true), kwargs...) if direct te isa ContinuousType && error("direct = true only applies to discrete-time systems") - C = C*A + K = (R∞*C')/(R2 + C*R∞*C') # ared returns K for the indirect form, which is A*Kdirect + return E ? (K, R∞, p, args...) : K end - Matrix(lqr(te, A',C',R1,R2, args...; kwargs...)') + E ? (Matrix(Kt'), R∞, p, args...) : Matrix(Kt') end function lqr(sys::AbstractStateSpace, Q, R, args...; kwargs...) diff --git a/lib/ControlSystemsBase/test/test_synthesis.jl b/lib/ControlSystemsBase/test/test_synthesis.jl index 239cc6527..0defd7cd4 100644 --- a/lib/ControlSystemsBase/test/test_synthesis.jl +++ b/lib/ControlSystemsBase/test/test_synthesis.jl @@ -188,6 +188,8 @@ end R = I L = lqr(Discrete, A,B,Q,R) @test L ≈ [0.5890881713787511 0.7118839434795103] + L, S, p = lqr(Discrete, A,B,Q,R, extra=Val(true)) + @test p ≈ eigvals(A-B*L) sys = ss(A,B,C,0,Ts) L = lqr(sys, Q, R) @test L ≈ [0.5890881713787511 0.7118839434795103] From 65614f7ddf14136947315b13c6318ae2ec1eeb5f Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 16 May 2025 12:19:33 +0200 Subject: [PATCH 2/2] fix comment --- docs/src/index.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/docs/src/index.md b/docs/src/index.md index 9df7296a3..09ff0e96b 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -99,5 +99,4 @@ The following is a list of packages from the wider Julia ecosystem that may be o ## Courses using ControlSystems.jl - [Carnegie Mellon University: Optimal-Control](https://github.com/Optimal-Control-16-745) - [Kasetsart University: Control Engineering with Julia](https://dewdotninja.github.io/julia/control/julia_control.html) -- [Czech Technical University: Optimal and Robust Control](https://hurak.github.io/orr/). [github repo](https://github.com/hurak/orr/tree/main) -- +- [Czech Technical University: Optimal and Robust Control](https://hurak.github.io/orr/). See also [github repo](https://github.com/hurak/orr/tree/main).