16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde Lecture 25 K2=PH(HPH+R) If ==x+y H=l K2=P(P-+R Then if p-≤R, Alternatively,ifR≤P, K,→I The effect is that 式=+K(-所) The quantity K, is known as the Kalman gain. It is the optimum gain in the mean squared error sense. Substitute it into the expression for P P*=(I-KH)P-(/-KH)+KRK (I-KH)P--(I-KHP-HK+KRK (I-KH)P--P-H'K+KHP-H'K+KRK (-KH)P--PHK+K(HPH +R)KT (-KH)P--P-H'K+ P-H(HP-HT+)(HP-HT+R)K (I-KH)P--P-H'K+P-H'K (I-KH)P- The form at the top is true for any choice of K. The last form is true only for the alman gain. The first form is better behaved numerically if you process a measurement which is very accurate relative to your prior information. So the measurement update step is Page 1 of 9
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 1 of 9 Lecture 25 Last time: ( ) 1 2 T T K P H HP H R − − − = + If k k z xv = + , ( ) 1 2 H I K PP R − − − = = + Then if P R − , 1 K PR 2 → − − Alternatively, if R P− , K I 2 → The effect is that xˆˆ ˆ x K z Hx ( ) +− − =+ − The quantity K2 is known as the Kalman gain. It is the optimum gain in the mean squared error sense. Substitute it into the expression for P+ . ( )( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) 1 T T TT T TT TT T TT T T TT T T T T TT TT P I KH P I KH KRK I KH P I KH P H K KRK I KH P P H K KHP H K KRK I KH P P H K K HP H R K I KH P P H K P H HP H R HP H R K I KH P P H K P H K I KH P + − − − −− − −− − − −− − − − −− − − =− − + =− −− + =− − + + =− − + + =− − + + + =− − + = − The form at the top is true for any choice of K . The last form is true only for the Kalman gain. The first form is better behaved numerically if you process a measurement which is very accurate relative to your prior information. So the measurement update step is:
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander Velde K=PHT(HP-HT+R) =红+k(-压) P*=(I-KH)P The filter operates by alternating time propagation and measurement update steps The results were derived here on the basis of preserving zero mean errors and minimizing the error variances. If all errors and noises are assumed normally distributed, so the probability density functions can be manipulated, one can derive the same results using the conditional mean approach: define x at every stage to be the mean of the distribution of x conditioned on all the measurements available up to that stage Example: Conversion of continnous dynamics to discrete time form 2 10 1(0)=x2(0)=0 unit white noise x 010 We could do time propagation by integration(N=1) P=AP+ pa+BB Φ=e=I+At+1A2 00‖0 If this does not work, expand o=4,如0)=1(△M=2) Page 2 of 9
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 2 of 9 ( ) ( ) ( ) 1 ˆˆ ˆ T T K P H HP H R x x K z Hx P I KH P − − − +− − + − = + =+ − = − The filter operates by alternating time propagation and measurement update steps. The results were derived here on the basis of preserving zero mean errors and minimizing the error variances. If all errors and noises are assumed normally distributed, so the probability density functions can be manipulated, one can derive the same results using the conditional mean approach: define x at every stage to be the mean of the distribution of x conditioned on all the measurements available up to that stage. Example: Conversion of continuous dynamics to discrete time form N 1 2 2 10 2 0 10 0 00 2 A B x x x n x x n = = ⎡ ⎤ ⎡⎤ = + ⎢ ⎥ ⎢⎥ ⎣ ⎦ ⎣⎦ We could do time propagation by integration (N = 1): ˆ ˆ T T x Ax P AP PA BB = =+ + ( )2 1 2 ... 2 A t e I At A t ∆ Φ= = + ∆ + ∆ + 2 0 10 0 10 0 0 0 0 0 0 00 A ⎡ ⎤⎡ ⎤ ⎡ ⎤ = = ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎣ ⎦⎣ ⎦ ⎣ ⎦ If this does not work, expand φ = Aφ , φ(0) = I (∆t = 2) :
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde 10△t Φ=I+AMt= 120 01 0=o(2, r)BNB'ap (2, t)'di (2,r)=+A(2-r) 110(2-r 120-10r ΦBNBp3=/40-20r [40-20x2 1600-1600r+400280-40r 80-40r 4 3200 ΦBNBΦdr=2 =O R=4 The initial conditions are given to be 0,so P(0)= Using the discrete formulation Time propagation: x+=①x P-1=ΦPΦ+Q measurement update K=P-H(HP-H+R =+K(=- Page 3 of 9
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 3 of 9 [ ] 1 10 0 1 1 20 0 1 1 0 k k t I At H ⎡ ⎤ ∆ Φ = + ∆= ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ = () () () ( ) ( ) 2 0 2, 2, 2, 2 1 10 2 1 20 10 0 1 0 1 T T Q BNB d k I A τ ττ τ τ τ τ =Φ Φ Φ =+ − ⎡ ⎤ − ⎡ ⎤ − = = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ ⎣ ⎦ ∫ [ ] 2 2 0 40 20 40 20 2 2 1600 1600 400 80 40 80 40 4 3200 80 2 80 8 4 T T T T k k BNB BNB d Q R τ τ τ τ τ τ τ ⎡ ⎤ − Φ Φ= − ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ −+ − = ⎢ ⎥ ⎣ ⎦ − ⎡ ⎤ Φ Φ= = ⎢ ⎥ ⎢ ⎥ ⎣ ⎦ = ∫ The initial conditions are given to be 0, so 0 ˆ (0) 0 0 0 (0) 0 0 x P − − ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ ⎡ ⎤ = ⎢ ⎥ ⎣ ⎦ Using the discrete formulation: Time propagation: 1 1 ˆ ˆ k k T k k x x PPQ − + + − + + = Φ =Φ Φ + Measurement update: ( ) ( ) ( ) 1 ˆˆ ˆ T T K P H HP H R x x K z Hx P I KH P − − − +− − + − = + =+ − = −
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde Continuous time filter As noted in the beginning of this section, we rarely process measurements ontinuously in a real system. However, there may be cases in which the measurements are processed so rapidly that one can approximate the process as tegration of a continuous time differential equati Also, since integration of a single differential equation for the estimate and one for the covariance matrix is logically simpler than alternating time propagation and measurement update steps, we sometimes approximate the hybrid continuous-discrete filter with a nearly equivalent continuous filter for analysis The relationship between continuous and discrete measurement processing can be seen in the following. Suppose we had the continuous measurement 三(1)=H(1)x(1)+y(1 where v(t) is an unbiased white noise process. Define a nearly equivalent discrete measurement to be the average of the continuous measurement over an interval Er(y()'=R(S(t-T tk (1)d [H(1)x()+y(1)d (4)x()△+Jy Hx()+」y(
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 4 of 9 Continuous time filter As noted in the beginning of this section, we rarely process measurements continuously in a real system. However, there may be cases in which the measurements are processed so rapidly that one can approximate the process as integration of a continuous time differential equation. Also, since integration of a single differential equation for the estimate and one for the covariance matrix is logically simpler than alternating time propagation and measurement update steps, we sometimes approximate the hybrid continuous-discrete filter with a nearly equivalent continuous filter for analysis purposes. The relationship between continuous and discrete measurement processing can be seen in the following. Suppose we had the continuous measurement zt Htxt vt () () () () = + where v t( ) is an unbiased white noise process. Define a nearly equivalent discrete measurement to be the average of the continuous measurement over an interval. () ( ) () ( ) T E vtv Rt t ⎡ ⎤ τ = − δ τ ⎣ ⎦ [ ] 1 1 1 1 1 ( ) 1 () () () 1 1 ( ) ( ) () 1 ( ) () k k k k k k k k t k t t t t k k t t k k t z ztd t H t x t v t dt t H t x t t v t dt t t H x t v t dt t τ − − − − = ∆ = + ∆ ≈ ∆+ ∆ ∆ = + ∆ ∫ ∫ ∫ ∫
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander Velde for small At. The equivalent discrete measurement noise is v(odt with v=0 since v(o=0, and its covariance is R=E[] dh2y(1)y(2) dJaR(4)6(2-4) dL, r(t) for small△t. o when we approximate a discrete measurement by a continuous one, the power density to assign to the continuous measurement noise is R(1)=R This makes sense because for larger Af(the discrete update process uses fewer measurements)the intensity of the continuous measurement noise is larger(the measurements are not as good) The discrete measurement gain is Kk=PH(HPH+R) Using the relation between Rk and R(t) we have R ote the difference in unit Discrete=kA(=-压)
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 5 of 9 for small ∆t . The equivalent discrete measurement noise is 1 1 ( ) k k t k t v v t dt t − = ∆ ∫ with 0 k v = since v t() 0 = , and its covariance is ( ) 1 1 1 1 1 2 1 21 2 2 1 2 1 21 2 1 1 2 1 ()( ) 1 ( ) 1 ( ) 1 ( ) ( ) k k k k k k k k k k T k kk t t T t t t t t t t t k k R E vv dt dt v t v t t dt dt R t t t t dt R t t Rt t t R t t δ − − − − − = ⎡ ⎤ ⎣ ⎦ = ∆ = − ∆ = ∆ ≈ ∆ ∆ = ∆ ∫ ∫ ∫ ∫ ∫ for small ∆t . So when we approximate a discrete measurement by a continuous one, the power density to assign to the continuous measurement noise is ( ) R k t Rt = ∆ This makes sense because for larger ∆t (the discrete update process uses fewer measurements) the intensity of the continuous measurement noise is larger (the measurements are not as good). The discrete measurement gain is ( ) 1 T T K P H HP H R k k − − − = + Using the relation between Rk and R( )t we have 1 ( ) T T k R t K P H HP H t − − − ⎛ ⎞ = + ⎜ ⎟ ⎝ ⎠ ∆ Note the difference in units: Discrete: xˆ ˆ ...K z Hx k k ( ) + − = −
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander Velde Continuous:i=Ai+Gu+K(=-Hi Then the continuous Kalman gain K(o=limK P-H'HP-H R() = Im =lmPH(△HPH+R(t =PA R =PH R The distinction between P- and p disappears as we go to the limit of The continuous-discrete update relations for i and P can be analyzed in a similar manner, expanding everything to first order in f and taking the limit a Ar-0. This is done in sufficient detail in the text. The result is the continuous time form of the Kalman filter 文=A+G+K(-压) P= AP+ Pa+BNB-PHR-HP where K= Ph R If this is an approximation to a filter that processes measurements at discrete points in time, with measurement noise variance R,, take R(O=R,At If the app nation is good the discrete and continuous variances will be related as
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 6 of 9 Continuous: ˆˆ ˆ ( ) k x =++ − Ax Gu K z Hx Then the continuous Kalman gain is ( ) 0 1 0 1 0 1 1 1 ( ) lim 1 () lim lim ( ) k t T T t T T t T T Kt K t R t P H HP H t t P H tHP H R t PH R PH R ∆ → − − − ∆ → − − − ∆ → − − − = ∆ ⎛ ⎞ = + ⎜ ⎟ ∆ ∆ ⎝ ⎠ = ∆+ = = The distinction between P− and P+ disappears as we go to the limit of continuous measurement processing. The continuous-discrete update relations for xˆ and P can be analyzed in a similar manner, expanding everything to first order in ∆t and taking the limit as ∆ →t 0 . This is done in sufficient detail in the text. The result is the continuous time form of the Kalman filter. ( ) 1 ˆˆ ˆ T TT x Ax Gu K z Hx P AP PA BNB PH R HP − =++ − =+ + − where T 1 K PH R− = . If this is an approximation to a filter that processes measurements at discrete points in time, with measurement noise variance Rk , take ( ) R k t Rt = ∆ If the approximation is good, the discrete and continuous variances will be related as
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde Example: Discrete time system(typical servo structure) 1 The input u(n) is any known command signal The noise n(n) is a wideband noise approximated as white with power density n The measurements are of y and are processed at intervals At with discrete measurement variance Rk Form the approximating continuous Kalman filter R=R2△ x1=x2 i2=KF(u-x1)-K(2+ .- =Ax+Gu+ Bn The continuous approximation measurement z is
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 7 of 9 Example: Discrete time system (typical servo structure) The input u t( ) is any known command signal. The noise n t( ) is a wideband noise approximated as white with power density N . The measurements are of y and are processed at intervals ∆t with discrete measurement variance Rk . Form the approximating continuous Kalman filter: R = ∆ R t k ( )( ) 1 2 2 12 01 0 0 F R FR F R x x x Kux Kx n x xu n KK K K Ax Gu Bn = = −− + ⎡ ⎤ ⎡⎤ ⎡ ⎤ = ++ ⎢ ⎥ ⎢⎥ ⎢ ⎥ ⎣ ⎦ ⎣⎦ ⎣ ⎦ −− − =++ The continuous approximation measurement z is
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde 0 Hx+y The filter is 文=A+Gm+K(x-压) P= AP+ pa+ BNB'-PhR-HP where K= Ph R Steady state properties If the system is time invariant and the random processes are stationary (A, B, H, N, R all constants), then the differential equation for P may approach a constant steady state value as time increases Some characteristics of the steady state solution: D)If(A, H) is detectable and(A, B) is stabilizable, the solution of the ricatti differential equation approaches a steady state value Po which independent of the initial condition Po 2) Under the conditions of (1), this P is the unique positive semidefinite solution of the algebraic Ricatti equation AP+PA+BNB'-PHR-HP=o 3)The steady state filter i=(A-KH)i+ Gu+K2 is asymptotically stable if and only if(A, H)is detectable at least and(A, B)is stabilizable at least with K=ph r- Page 8 of 9
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 8 of 9 z xv [1 0] Hx v = + = + The filter is: ( ) 1 ˆˆ ˆ T TT x Ax Gu K z Hx P AP PA BNB PH R HP − =++ − =+ + − where T 1 K PH R− = . Steady state properties If the system is time invariant and the random processes are stationary ( ABH N R ,, , , all constants), then the differential equation for P may approach a constant steady state value as time increases. Some characteristics of the steady state solution: 1) If (, ) A H is detectable and (, ) A B is stabilizable, the solution of the Ricatti differential equation approaches a steady state value P∞ which is independent of the initial condition P0 . 2) Under the conditions of (1), this P∞ is the unique positive semidefinite solution of the algebraic Ricatti equation 1 0 T TT AP PA BNB PH R HP − ++ − = 3) The steady state filter xˆ ˆ =− + + ( ) A KH x Gu Kz is asymptotically stable if and only if ( A H, ) is detectable at least and ( ) A B, is stabilizable at least with T 1 K PH R− = ∞
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander Velde 4)Under the conditions of (3), the steady state filter minimizes limE(n) Me(n) for every M>0. This value is [P2M] 5) Po is positive definite if and only if(A, B)is complete controllable 6)If(A, H)is observable, steady state P is positive definite Page 9 of 9
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 9 of 9 4) Under the conditions of (3), the steady state filter minimizes lim ( ) ( ) T t E e t Me t →∞ ⎡ ⎤ ⎣ ⎦ for every M > 0 . This value is tr[P M∞ ] 5) P∞ is positive definite if and only if ( A B, ) is complete controllable. 6) If ( ) A, H is observable, steady state P is positive definite
