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 ( ) + − = −