Steady State Kalman Filter

Transcription

1 Steady State Kalman Filter Infinite Horizon LQ Control: ẋ = Ax + Bu R positive definite, Q = Q T 2Q 1 2. (A, B) stabilizable, (A, Q 1 2) detectable. Solve for the positive (semi-) definite P in the ARE: A T P + P A P BR 1 B T P + Q = 0. Optimal feedback gain: u = Kx K = R 1 B T P A BK stable. Steady State Kalman Filter: ˆx = Aˆx + Bu + L(y Cˆx) Choose L so that A LC or A T C T L is stable. So solve LQ control problem for the dual problem: ẋ = A T x + C T u (60) M.E. University of Minnesota 276

2 Transform the Algebraic Riccati Equation: C T B, A T A. AP + P A T P C T R 1 CP + Q = 0. Stabilizing feedback gain K for (60) is: K = L T = R 1 CP L = P C T R 1 This generates the observer: ˆx = Aˆx + Bu + P C T R 1 (y Cˆx) with observer error dynamics: x = (A P C T R 1 C) x which will be stable, as long as 1. (A T, C T ) is stabilizable. This is the same as (A, C) being detectable. 2. (A T, Q 1 2) is detectable. This is the same as (A, Q 1 2) being stabilizable. M.E. University of Minnesota 277

3 Questions we need to ask are: In what way is this observer optimal? How does finite time LQ extend to observer design? M.E. University of Minnesota 278

4 Deterministic Kalman Filter Problem statement: Suppose we are given the output y(τ) of a process over the interval [t 0, t f ]. It is anticipated that y(τ) is generated from a process of the form: ẋ = A(t)x + B(t)u y = C(t)x To account for model uncertainty and measurement noise, the process model used is modified to: ẋ(t) = A(t)x(t) + B(t)u + B w (t)w y = C(t)x(t) + v (61) The state ˆx(t t f ) = estimate of the state of the actual process at time t given all the measurements up to time t f. ˆx(t t f ) means derivative w.r.t. t. The final time is assumed to be fixed. In (61), w and v are unknown process and measurement noise. M.E. University of Minnesota 279

5 Initial state ˆx(t 0 t f ) unknown. The deterministic Kalman filter problem is to so choose the initial state ˆx(t 0 t f ) and the measurement and process noises w(τ t f ), v(τ t f ) for τ [t 0, t f ], so that: 1. The measured output is explained: i.e. y(τ) = C(τ)ˆx(τ t f ) + v(τ t f ) for all τ [t 0, t f ]. 2. Minimize the following objective function: J(y( ), ˆx(t 0 t f ), w( t f )) = 1 2ˆxT (t 0 t f )S 1ˆx(t 0 t f ) tf t 0 w T (τ t f )Q 1 (τ)w(τ t f ) + v T (τ t f )R 1 (τ)v(τ t f ) dτ where v(τ t f ) = y(τ) C(τ)x(τ t f ). M.E. University of Minnesota 280

6 Remarks: Thus, the goal is to use as little noise or initial condition as possible so as to explain the measured output y(τ). Set R 1 large when measurement is accurate, i.e. assume little measurement noise v. Set Q 1 large when process model is accurate, i.e. assume little process noise w. Set S 1 large when estimate of ˆx(t 0 t f ) = 0 is a confident guess of the initial state. If a non-zero estimate (say x 0 ) for the initial state is desired, it is possible to modify the cost function so that: 1 2ˆxT (t 0 t f )S 1ˆx(t 0 t f ) 1 2 (ˆx(t 0 t f ) x 0 ) T S 1 (ˆx(t 0 t f ) x 0 ). It is clear that the optimal estimate at t 0 given no measurements is: ˆx(t 0 t 0 ) = x 0. M.E. University of Minnesota 281

7 In the observer problem, we are interested in ˆx(t t) where t is the current time. Thus, we need to solve ˆx(t f t f ) and then determine how this varies with t f. Conceptually, we need to resolve this problem when t f changes. But, in practice, this is not necessary. Filtering problem is to solve ˆx(t t f ) where t < t f. Prediction problem is to solve ˆx(t t f ) where t > t f. When solving the problem, we drop the B(t)u term in ẋ = A(t)x + B(t)u for convenience. It can be carried through. M.E. University of Minnesota 282

8 Optimal filtering / observer Problem The free variables in the optimal filtering / observer problem are: ˆx(t 0 t f ), the estimate of the initial state and process noise w(τ). Notice that once these have been chosen, v(τ t f ) = y(τ) C(τ)ˆx(τ t f ) is fixed. Let the final time t f be fixed. We omit the final time argument t f. Use ˆx(τ) to denote ˆx(τ x f ) etc. The constraint of the system is: ˆx(τ) = Aˆx(τ) + w(τ), τ [t 0, t f ]. We can convert this constrained optimal control problem into unconstrained optimization problem using the Lagrange multiplier λ(τ) R n. The cost function for the unconstrained optimization M.E. University of Minnesota 283

9 problem is: J = 1 2ˆxT (t 0 )S 1ˆx(t 0 ) tf t 0 w T Q 1 (τ)w +v T R 1 (τ)v + 2λ T (τ)[ ˆx (A(τ)ˆx + w)] dτ Using integration by parts and considering variation in ˆx(τ), w(τ), ˆx(t f ) and ˆx(t 0 ). δ J = tf t 0 [ w T (τ)q 1 (τ) λ T (τ) ] δw(τ)+ [ v T (τ)r 1 (τ)c T (τ) λ ] T (τ) λ T (τ)a(τ) + λ T (t f )δˆx(t f ) + [ˆx(t 0 )S 1 λ T (t 0 ) ] δˆx(t 0 ). δˆx(τ) dt Since δx(τ), δw(τ), δˆx(t 0 ) and δˆx(t f ) can be arbitrary, as long as ˆx = A(τ)ˆx + w, the necessary condition for optimality is: δw(τ) w(τ) = Q(τ)λ(τ) δˆx(τ) λ(τ) = A T (τ)λ(τ) C T (τ)r 1 (τ)[y(τ) C(τ)ˆx(τ)] M.E. University of Minnesota 284

10 So that the differential equations are: ( ) ˆẋ λ = ( A Q T)(ˆx ) C T R 1 C A λ and the boundary conditions are: ( ) 0 C T R 1 y(τ) (62) δˆx(t f ) λ(t f ) = 0 δˆx(t 0 ) λ(t 0 ) = S 1ˆx(t 0 ) This is a two point boundary value problem. Notice that there is some similarity to the Hamilton- Jacobi-Bellman equation we saw for LQ control problem. We solve this using a similar technique as for LQ to resolve the two point boundary value problem. Assume that ˆx(t) = P(t)λ(t) + g(t) (63) Here g(t) is used to account for the exogenous term C T (τ)r 1 (τ)y(τ). For (63) to be consistent with the HJB equations and M.E. University of Minnesota 285

11 boundary conditions, we need: P = A(τ)P + PA T (τ) + Q(τ) PC T (τ)r 1 (τ)c(τ)p (64) ġ = A(τ)g P(τ)C T (τ)r 1 (τ)[c(τ)g y(τ)] (65) with initial condition g(t 0 ) = 0 and P(t 0 ) = S. The first differential equation is the Filtering Riccati Differential Equation, which reduces to the CTRDE for LQ control if A A T, C B T and τ (t f τ). Notice that all boundary conditions are assigned at τ = t 0. Thus, (64)-(65) can be integrated forward in time. Substituting (63) into (62), λ = (A T C T R 1 CP)λ + C T R 1 [Cg(t) y]; (66) with boundary condition λ(t f ) = 0. λ(t) can be integrated backwards in time, using P(t) and g(t) obtained from (64)-(65). M.E. University of Minnesota 286

12 Once λ(t) is obtained, the optimal estimate of the state at τ = t is then given by: ˆx(t t f ) = P(t t f )λ(t t f ) + g(t t f ). Here we re-inserted the argument t f to emphasize that the measurements y(t) is available on the interval [t 0, t f ]. M.E. University of Minnesota 287

13 Det. Kalman Filter: Summary Forward pass differential equations P = A(τ)P + PA T (τ) + Q(τ) PC T (τ)r 1 (τ)c(τ)p ġ = A(τ)g P(τ)C T (τ)r 1 (τ)[c(τ)g(τ) y(τ)] Backward pass differential equation λ = (A T C T R 1 CP)λ + C T R 1 [C(τ)g(τ) y(τ)]. Boundary conditions: g(t 0 ) = 0, P(t 0 ) = S λ(t f ) = 0. State estimate: ˆx(t t f ) = P(t t f )λ(t t f ) + g(t t f ). P(t) and g(t) are integrated first (forward in time). We will see that g(t) is an estimate of ˆx(t t). The backward integration of λ updates the estimate of ˆx(t t) using future information. M.E. University of Minnesota 288

14 Deterministic Optimal Observer Let us denote P(t t f ) and g(t t f ) as the solutions to (64)-(65) with the available measurement upto t f. Then, the solution ˆx(t t) is given by taking t f = t, ˆx(t t) = P(t t)λ(t t) + g(t t) = g(t t) since λ(t t) = λ(t f ) = 0. Now, since the boundary conditions for (64)-(65) are given at t 0, so for any > 0, P(t t f ) = P(t t f + ); g(t t f ) = g(t t f + ) Hence, as t f increases, the solution for P(t t f ) and g(t t f ) can be resused. In fact, the terminal time t f does not enter into the solution of P( ) or g( ). M.E. University of Minnesota 289

15 This means that the optimal observer is given by: d t) = ġ(t) dtˆx(t =A(t)g(t) P(t)C T (t)r 1 (t)[c(t)g(t) y(t)] (67) =Aˆx(t t) P(t)C T (t)r 1 (t)[c(t)ˆx(t t) y(t)]; }{{} L(t) (68) with initial condition ˆx(t 0 t 0 ) = x 0 = 0, and P = A(t)P + PA T (t) + Q(t) PC T (t)r 1 (t)c(t)p; P(t 0 ) = S and L(t) is the observer gain. The Filtering Riccati Equation becomes the control Riccati Equation: A T A, C T B, τ (t f τ) The asymptotic solution P(t ) for the Kalman Filtering problem is analogous to P(t 0 ) for the LQ control problem. Asymptotic properties of the Kalman filter can be obtained using this analogy. M.E. University of Minnesota 290

16 The ARE to be solved is obtained using the above substitutions: AP + P A T + Q P C T (t)r 1 CP = 0. In the time invariant case, P(t ) converges if (A, C) is detectable. If (A T, Q 1 2) is detectable, i.e. stabilizable, then (A, Q 1 2 T ) is A c = A P C T R 1 C is stable. Notice that although Q 1 is used in the performance function, the Riccati equation uses Q. So, it is possible to implement a Kalman filter with Q not invertible. In this case, this means that process noise does not affect each state directly: ẋ = Ax + B w w The matrix Q 1 in the performance criteria can be modified accordingly. M.E. University of Minnesota 291

17 Stochastic Kalman Filter Consider a stochastic process: ẋ = Ax + w; x(t 0 ) = x 0 y = Cx + v (69) Here w(t) and v(t) are random noise, and x 0 is a random variable: such that E[w(t)w T (τ)] = Q(t)δ(t τ) (70) E[v(t)v T (τ)] = R(t)δ(t τ) (71) E[v i (t)w j (τ) = 0 (72) E[x 0 x T 0 ] = S (73) where E(x) is the expected value (mean) of the random variable x. In other words, w(t) is un-related to w(τ) whenever τ t. v(t) is un-related to v(τ) whenever τ t. v(t) is not related to w(τ) at all. M.E. University of Minnesota 292

18 Q(t) is the expected size of w(t)w T (t). R(t) is the expected size of v(t)v T (t). S is the expected size of x 0 x T 0. Assume that y(τ) is measured for τ [t 0, t]. If we apply the deterministic Kalman Filter to this stochastic process to obtain ˆx(t t), and let x(t) = ˆx(t t) x(t). Then, it turns out that this estimate minimizes: E[ x(t) x T (t)] Moreover, the solution to the Filtering Riccati Equation is P(t) = E[ x(t) x T (t)] See Goodwin Chapter for the derivation of this result. M.E. University of Minnesota 293

19 Pink Noise Noise does not uniformly cover a broad frequency spectrum, but instead is concentrated in some bands, are pink. They can be modeled by the output of a bandpass filter with input of white noise. w pink (s) = F(s)w white (s) In states space, where ẋ w = A p x w + B p w white w pink = C p x w + D p w white F(s) = C p (si A p ) 1 B p + D p The states space model is then incorporated into the plant equation with the augmented state, with white noise as inputs. Procedure is applied for both process noise or output measurement nosie. M.E. University of Minnesota 294

20 Linear Quadratic Gaussian (LQG) Consider the system driven by Gaussian noise: where ẋ = Ax + Bu + w y = Cx + v E[w T (t)w(τ)] = Q w (t)δ(t τ) (74) E[v T (t)v(τ)] = R w (t)δ(t τ) (75) E[v i (t)w j (τ) = 0 (76) E[x 0 x T 0 ] = S w (77) Moreover, assume that the probability distributions of w and v are zero mean, and Gaussian. Suppose that one wants to minimize: J = E where T. { 1 T t0 +T t 0 [ x T Qx + u T Ru ] dt } M.E. University of Minnesota 295

21 This is called the Linear Quadratic Gaussian (LQG) problem. It turns out that the optimal control in this sense is given by: u = Kˆx where K is the gain obtained by considering the LQR problem. ˆx is the state estimate obtained from using the Kalman Filter. From the separation theorem, we know that the observer estimated state feedback generates a stable controller such that the closed loop system has eigenvalues given by the eigenvalues of the observer error and the eigenvalues of the state feedback controller. The LQG result shows that Kalman Filter and LQ generates an Optimal Controller in the stochastic sense. The LQG controller is optimal over ALL causal, linear and nonlinear controllers! M.E. University of Minnesota 296