AN5018. Basic Kalman Filter Theory. Document information. This document derives the standard Kalman filter equations.

Transcription

1 Rev June 2016 Application note Document information Info Content Abstract This document derives the standard Kalman filter equations.

2 Revision history Document ID Release date Supercedes v v1.0 Modifications: Minor changes The format of this document has been redesigned to comply with the new identity guidelines of NXP Semiconductors. Legal texts have been adapted to the new company name where appropriate. v September Contact information For more information, please visit: All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

3 1. Introduction This document derives the standard Kalman filter equations. It is intended as a primer that should be read before tackling Application note AN5023 Sensor Fusion Kalman Filters which describes the more specialized indirect complementary Kalman filter used for the fusion of accelerometer, magnetometer and gyroscope data in the NXP Sensor Fusion Library software. Section 2 calculates some mathematical results used in the derivation. The derivation itself is in Section Terminology Symbol AA kk Definition The linear prediction or state matrix at sample kk. xx kk = AA kk xx kk1 ww kk xx kk = AA kk xx kk1 CC kk The measurement matrix relating zz kk to xx kk at sample kk. zz kk = CC kk xx kk vv kk EE[] KK kk PP kk Expectation operator The Kalman filter gain matrix at sample kk. The a priori covariance matrix of the linear prediction (a priori) error xx εε,kk at sample kk. PP kk = EE xx εε,kk xx TT εε,kk PP kk The a posteriori covariance matrix of the Kalman (a posteriori) error xx εε,kk at sample kk. PP kk = EE xx εε,kk xx TT εε,kk QQ ww,kk The covariance matrix of the additive noise ww kk in the process xx kk. QQ ww,kk = EE[ww kk ww kk TT ] QQ vv,kk The covariance matrix of the additive noise vv kk in the measured process zz kk. QQ vv,kk = EE[vv kk vv kk TT ] VV[] Variance operator All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

4 Symbol vv kk ww kk Definition The additive noise in the measured process zz kk at sample kk. The additive noise in the process of interest xx kk at sample kk. xx kk The state vector at time sample kk of the process xx kk. xx kk = AA kk xx kk1 ww kk xx kk xx kk The linear prediction (a priori) estimate of the process xx kk at sample kk. xx kk = AA kk xx kk1 The Kalman filter (a posteriori) estimate of the process xx kk at sample kk. xx kk = (II KK kk CC kk )xx kk KK kk zz kk = (II KK kk CC kk )AA kk xx kk1 KK kk zz kk xx εε,kk The error in the linear prediction (a priori) estimate of the process xx kk. = xx kk xx kk xx εε,kk xx εε,kk The error in the a posteriori Kalman filter estimate of the process xx kk. xx εε,kk = xx kk xx kk zz kk The measured process at sample kk. zz kk = CC kk xx kk vv kk δδ kk,jj The Kronecker delta function. δδ kk,jj = 1 for kk = jj and zero otherwise. 2. Mathematical Lemmas 2.1 Lemma 1 The trace of the sum of two square matrices AA and BB equals the sum of the individual traces. The proof is trivial. NN1 tttt(aa BB) = AA iiii BB iiii = AA iiii BB iiii = tttt(aa) tttt(bb) (1) ii=0 NN1 ii=0 NN1 ii=0 All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

5 2.2 Lemma 2 The derivative with respect to AA of the trace of the matrix product CC = AAAA equals BB TT. {tttt(cc)} AA = {tttt(aaaa)} AA (AAAA) (AAAA) (AAAA) AA 0,0 AA 0,1 AA 0,NN1 = (AAAA) (AAAA) (AAAA) AA 1,0 AA 1,1 AA 1,NN1 (AAAA) AA MM1,0 (AAAA) AA MM1,1 (AAAA) AA MM1,NN1 (2) Proof: If the matrix AA has dimensions MM NN and the matrix BB has dimensions NN MM, then CC = AAAA has dimensions MM MM. The element CC iiii of matrix CC has value: NN1 MM1 MM1 NN1 CC iiii = AA iiii BB kkkk tttt(cc) = tttt(aaaa) = CC iiii = AA iiii BB kkkk kk=0 ii=0 ii=0 kk=0 (3) Substituting equation (3) into equation (2) gives: {tttt(aaaa)} AA MM1 ii=0 NN1 kk=0 MM1 ii=0 NN1 kk=0 MM1 ii=0 NN1 kk=0 AA iiiibb kkkk AA iiiibb kkkk AA iiiibb kkkk AA 0,0 AA 0,1 AA 0,NN1 = MM1 NN1 ii=0 kk=0 AA iiiibb kkkk MM1 NN1 ii=0 kk=0 AA iiiibb kkkk MM1 NN1 ii=0 kk=0 AA iiiibb kkkk AA 1,0 AA 1,1 AA 1,NN1 MM1 NN1 kk=0 AA iiiibb kkkk ii=0 AA MM1 NN1 kk=0 AA iiiibb kkkk MM1,0 ii=0 AA MM1 NN1 ii=0 kk=0 AA iiiibb kkkk MM1,1 AA MM1,NN1 (4) By inspection: MM1 NN1 ii=0 kk=0 AA iiiibb kkkk = BB AA mmmm (5) llll Substituting equation (5) into equation (4) completes the proof: BB 0,0 BB 1,0 BB NN1,0 {tttt(aaaa)} BB = 0,1 BB 1,1 BB NN1,1 = AA BBTT (6) BB 0,MM1 BB 1,MM1 BB NN1,MM1 All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

6 2.3 Lemma 3 The derivative with respect to AA of the trace of the matrix product AAAAAA TT equals AA(BB BB TT ). {tttt(aaaaaa TT )} AA ) (AAAAAATT (AAAAAATT ) (AAAAAATT ) AA 0,0 AA 0,1 AA 0,NN1 = (AAAAAATT ) (AAAAAATT ) (AAAAAATT ) AA 1,0 AA 1,1 AA 1,NN1 (AAAAAATT ) (AAAAAATT ) (AAAAAATT ) AA MM1,0 AA MM1,1 AA MM1,NN1 (7) Proof: If the matrix AA has dimensions MM NN, then the matrix BB must be square with dimensions NN NN in order for the product AAAAAA TT to exist. The product AAAAAA TT is always square with dimensions MM MM. The element CC iiii of the matrix CC = AAAA has value: NN1 CC iiii = AA iiii BB kkkk kk=0 (8) The element DD iiii of matrix DD = AAAAAA TT = CCAA TT has value: NN1 NN1 NN1 DD iiii = CC iiii AA llll = AA iiii BB kkkk AA llll jj=0 jj=0 kk=0 (9) The trace of matrix DD then equals: NN1 NN1 NN1 NN1 tttt(dd) = DD iiii = AA iiii BB kkkk AA iiii ii=0 ii=0 jj=0 kk=0 (10) The derivative of tttt(dd) with respect to AA llll is: NN1 ii=0 NN1 jj=0 NN1 kk=0 tttt(dd) = AA iiiibb kkkk AA iiii = AA llllbb kkkk AA llll (11) AA llll AA llll AA llll NN1 NN1 jj=0 NN1 kk=0 = AA llll BB mmmm AA llll BB jjjj = (AABB TT ) llll (AAAA) llll (12) jj=0 NN1 jj=0 {tttt(aaaaaatt )} AA = AA(BB BB TT ) (13) All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

7 If BB is also symmetric, then: {tttt(aaaaaa TT )} = 2AAAA iiii BB = BB TT (14) AA 3. Kalman Filter Derivation 3.1 Process Model The Kalman filter models the vector process of interest xx kk with the linear and recursive model: xx kk = AA kk xx kk1 ww kk (15) If xx kk has NN degrees of freedom, then AA kk is an NN NN linear prediction matrix (possibly time varying but assumed known) and ww kk is an NN 1 zero mean white noise vector. The process xx kk is assumed to be not directly measurable and must be estimated from a process zz kk which can be measured. zz kk is modeled as being linearly related to xx kk with additive zero mean white noise vv kk. zz kk = CC kk xx kk vv kk (16) zz kk is an MM 1 vector, CC kk is an MM NN matrix (possibly time varying but assumed known) and vv kk is an MM 1 noise vector. Since the noise vectors wk and vk are zero-mean white noise processes their expectation vector is zero and their covariance matrices are uncorrelated at different times j and k: EE[ww kk ] = 00 (17) EE[vv kk ] = 00 (18) cccccc{ww kk, ww jj } = EE ww kk ww TT jj = QQ ww,kk δδ kkkk (19) cccccc{vv kk, vv jj } = EE vv kk vv TT jj = QQ vv,kk δδ kkkk (20) Covariance matrices are, by definition, symmetric but not necessarily diagonal: QQ ww,kk TT = {EE[ww kk ww kk TT ]} TT = EE[(ww kk ww kk TT ) TT ] = EE ww kk ww jj TT = QQ ww,kk (21) The covariance matrices QQ ww,kk and QQ vv,kk need not be stationary and can, and generally will, vary with time. All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

8 3.2 Derivation The objective of the Kalman filter is to compute an unbiased a posterori estimate xx kk of the underlying process xx kk from i) extrapolation from the previous iteration's a posteriori estimate xx kk1 and ii) from the current measurement zz kk : xx kk = KK kk xx kk1 KK kk zz kk (22) The time-varying Kalman gain matrices KK kk and KK kk define the relative weightings given to the previous iteration s Kalman filter estimate KK kk and to the current measurement zz kk. If the measurements zz kk have low noise then the measurement term KK kk zz kk will have a higher weighting compared to the extrapolated component KK kk xx kk1 and vice versa. The Kalman filter is, therefore, a time varying, recursive filter. Unbiased estimate constraint (determines KK kk ) For xx kk to be an unbiased estimate of xx kk, the expectation value of the a posteriori Kalman filter error xx εε,kk must be zero: EE xx εε,kk = EE[xx kk xx kk ] = 00 (23) Subtracting xx kk from equation (22) gives: xx εε,kk = xx kk xx kk = KK kk xx kk1 KK kk zz kk xx kk (24) Substituting equation (16) for the measurement zz kk gives: xx εε,kk = KK kk xx kk1 KK kk (CC kk xx kk vv kk ) xx kk (25) Substituting for xx kk from equation (15) and rearranging gives: xx εε,kk = KK kk xx εε,kk1 xx kk1 KK kk {CC kk (AA kk xx kk1 ww kk ) vv kk } (AA kk xx kk1 ww kk ) (26) = KK kk xx εε,kk1 (KK kk CC kk AA kk AA kk KK kk )xx kk1 (KK kk CC kk II)ww kk KK kk vv kk (27) Taking the expected value of equation (27) and applying the unbiased estimate constraint gives: EE xx εε,kk = EE KK kk xx εε,kk1 EE[(KK kk CC kk AA kk AA kk KK kk )xx kk1 ] EE[(KK kk CC kk II)ww kk ] EE[KK kk vv kk ] = 00 (28) Because the noise vectors ww kk and vv kk are zero mean and uncorrelated with the Kalman matrices for the same iteration, it follows that: EE[(KK kk CC kk II)ww kk ] = EE[KK kk vv kk ] = 00 (29) All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

9 With the additional assumption that the process xx kk1 is independent of the slowly varying matrices AA kk, CC kk, KK kk and KK kk at iteration kk: EE[(KK kk CC kk AA kk AA kk KK kk )xx kk1 ] = (KK kk CC kk AA kk AA kk KK kk )EE[xx kk1 ] = 00 (30) Because xx kk is not, in general, a zero-mean process: KK kk CC kk AA kk AA kk KK kk = 00 KK kk = AA kk KK kk CC kk AA kk = (II KK kk CC kk )AA kk (31) Substituting for KK kk in equation (22) gives: xx kk = (II KK kk CC kk )AA kk xx kk1 KK kk zz kk (32) A priori estimate The a priori Kalman filter estimate xx kk is the result of applying the linear prediction matrix AA kk to the previous iteration's a posteriori estimate xx kk1 : xx kk = AA kk xx kk1 Kalman equation (A) (33) Definition of a posteriori estimate Substituting the a priori estimate xx kk from equation (33) into equation (32) gives: xx kk = (II KK kk CC kk )xx kk KK kk zz kk Kalman equation (D) (34) An equivalent form is: xx kk = xx kk KK kk (zz kk CC kk xx kk ) (35) From equation (16), the term CC kk xx kk can be interpreted as the a priori estimate zz kk of the measurement zz kk giving another form of equation (34): xx kk = xx kk KK kk (zz kk zz kk ) (36) PP kk as a function of PP kk11 The a priori and a posteriori error covariance matrices PP kk and PP kk are defined as: PP kk = cccccc{xx εε,kk, xx εε,kk } = EE xx εε,kk xx TT εε,kk = EE[(xx kk xx kk )(xx kk xx kk ) TT ] (37) PP kk = cccccc{xx εε,kk, xx εε,kk } = EE xx εε,kk xx TT εε,kk = EE[(xx kk xx kk )(xx kk xx kk ) TT ] (38) All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

10 Substituting the definitions of xx kk and xx kk into equation (37) gives an expression relating the current a priori error covariance PP kk to the previous iteration's a posteriori error covariance estimate PP kk1 : PP kk = EE[(AA kk xx kk1 AA kk xx kk1 ww kk )(AA kk xx kk1 AA kk xx kk1 ww kk ) TT ] (39) = EE[{AA kk (xx kk1 xx kk1 ) ww kk }{AA kk (xx kk1 xx kk1 ) ww kk } TT ] (40) = AA kk EE[(xx kk1 xx kk1 )(xx kk1 xx kk1 ) TT ]AA TT kk QQ (41) ww,kk PP kk = AA kk PP kk1 AA TT kk QQ ww,kk Kalman equation (B) (42) Minimum error covariance constraint (determines KK kk ) The Kalman gain matrix KK kk minimizes the a posteriori error xx εε,kk variance via the trace of the a posteriori error covariance matrix PP kk : EE xx TT εε,kk xx εε,kk = tttt(pp kk ) (43) Substituting equation (16) for zz kk into equation (32) gives a relation between the a posteriori and a priori errors: xx kk = xx εε,kk xx kk = (II KK kk CC kk ) xx εε,kk xx kk KK kk (CC kk xx kk vv kk ) (44) xx εε,kk xx kk = (II KK kk CC kk )xx εε,kk xx kk KK kk CC kk xx kk KK kk (CC kk xx kk vv kk ) (45) xx εε,kk = (II KK kk CC kk )xx εε,kk KK kk vv kk (46) Substituting this result into the definition of the a posteriori covariance matrix PP kk in equation (38) gives: PP kk = EE (II KK kk CC kk )xx εε,kk KK kk vv kk (II KK kk CC kk )xx εε,kk KK kk vv kk TT (47) = (II KK kk CC kk )EE xx εε,kk xx TT εε,kk (II KK kk CC kk ) TT KK kk EE[vv kk vv TT TT kk ]KK (48) kk = (II KK kk CC kk )PP kk (II KK kk CC kk ) TT KK kk QQ vv,kk KK kk TT (49) = PP kk PP kk CC kk TT KK kk TT KK kk CC kk PP kk KK kk CC kk PP kk CC kk TT KK kk TT KK kk QQ vv,kk KK kk TT (50) The Kalman filter gain KK kk is that which minimizes the trace of the a posteriori error covariance matrix PP kk as in equation (43): tttt(pp KK kk ) = kk KK kk tttt(pp kk ) tttt PP kk CC kk TT KK kk TT tttt(kk kk CC kk PP kk ) tttt KK kk CC kk PP kk CC kk TT KK kk TT tttt KK kk QQ vv,kk KK kk TT = 0 (51) The first term tttt(pp kk ) has no dependence on KK kk giving: All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

11 {tttt(pp kk )} KK kk = tttt(aa kkpp kk1 AA TT kk QQ ww,kk ) = 0 (52) KK kk Because a matrix trace is obviously unaffected by transposition, the second term of equation (51) can be transposed and simplified using equation (6) to give: tttt(pp kk CC kk TT KK kk TT ) KK kk = {tttt(kk kkcc kk PP kk )} = (CC KK kk PP kk ) TT = PP TT kk CC (53) kk kk The fourth term can be simplified using equations (13) and (14) exploiting the fact that the covariance matrix PP kk is symmetric: tttt(kk kk CC kk PP kk CC kk TT KK kk TT ) KK kk = KK kk CC kk PP kk CC kk TT CC kk PP kk CC kk TT TT = 2KK kk CC kk PP kk CC kk TT (54) The final term can be simplified also using equations (13) and (14) and the symmetry of QQ vv,kk to give: tttt(kk kk QQ vv,kk KK kk TT ) KK kk = 2KK kk QQ vv,kk (55) Substituting equations (52) to (55) back into equation (51) gives an expression for the optimal Kalman filter gain matrix KK kk : 2PP kk CC kk TT 2KK kk CC kk PP kk CC kk TT 2KK kk QQ vv,kk = 00 (56) KK kk CC kk PP kk CC kk TT QQ vv,kk = PP kk CC kk TT (57) KK kk = PP kk CC kk TT CC kk PP kk CC kk TT QQ vv,kk 1 Kalman equation (C) (58) PP kk as a function of PP kk Rearranging equation (57) gives: KK kk QQ vv,kk = PP kk CC kk TT KK kk CC kk PP kk CC kk TT (59) All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

12 Substituting KK kk QQ vv,kk from equation (59) into equation (49) gives: PP kk = (II KK kk CC kk )PP kk II CC kk TT KK kk TT (II KK kk CC kk )PP kk CC kk TT KK kk TT (60) = PP kk KK kk CC kk PP kk PP kk CC kk TT KK kk TT KK kk CC kk PP kk CC kk TT KK kk TT PP kk CC kk TT KK kk TT KK kk CC kk PP kk CC kk TT KK kk TT (61) PP kk = (II KK kk CC kk )PP kk Kalman equation (E) (62) This completes the derivation of the standard Kalman filter equations. 3.3 Standard Kalman Equations This section simply re-lists the key Kalman filter equations derived in the previous section. Kalman equation (A) The linear prediction (a priori) estimate xx kk is made by applying the linear prediction matrix AA kk to the previous sample s Kalman (a posteriori) filter estimate xx kk1. xx kk = AA kk xx kk1 (A) Kalman equation (B) The a priori (linear extrapolation) error covariance matrix PP kk is then updated using the model matrix AA kk and the noise matrix QQ ww,kk. PP kk = AA kk PP kk1 AA TT kk QQ ww,kk (B1) Kalman equations (B) and (E) can be combined to give a recursive update of PP kk without explicit calculation of the a posteriori error covariance matrix PP kk in Kalman equation (E): PP kk = AA kk (II KK kk1 CC kk1 )PP kk1 AA TT kk QQ ww,kk (B2) The only purpose of PP kk is to permit the calculation of the Kalman gain matrix KK kk for the determination of the a posteriori estimate xx kk. Kalman equation (C) The Kalman filter gain matrix KK kk is updated: KK kk = PP kk CC kk TT CC kk PP kk CC kk TT QQ vv,kk 1 (C) All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

13 Kalman equation (D) The Kalman filter (a posteriori) estimate xx kk is computed from the current a priori estimate xx kk and the current measurement zz kk. xx kk = xx kk KK kk (zz kk CC kk xx kk ) = (II KK kk CC kk )xx kk KK kk zz kk (D) Kalman equation (E) The a posteriori Kalman error covariance matrix PP kk is updated and ready for the next iteration. This equation can be skipped if PP kk is updated recursively. PP kk = (II KK kk CC kk )PP kk (E) 3.4 Limiting Cases From equation (C), as the measurement noise covariance QQ vv,kk decreases relative to the process noise covariance QQ ww,kk, the Kalman gain matrix KK kk satisfies: KK kk CC kk PP kk CC kk TT = PP kk CC kk TT KK kk CC kk = II (63) Using equation (D), the a posteriori estimate xx kk is then only dependent on the measurement zz kk : xx kk = (II KK kk CC kk )xx kk KK kk zz kk = KK kk zz kk (64) As the measurement noise covariance QQ vv,kk increases relative to the process noise covariance QQ ww,kk, the Kalman gain matrix KK kk approaches zero: KK kk = PP kk CC kk TT QQ vv,kk 1 = 00 (65) The a posteriori process estimate xx kk then approximates the a priori estimate xx kk : xx kk = xx kk KK kk (zz kk CC kk xx kk ) = xx kk (66) All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

14 4. Legal information 4.1 Definitions Draft The document is a draft version only. The content is still under internal review and subject to formal approval, which may result in modifications or additions. NXP Semiconductors does not give any representations or warranties as to the accuracy or completeness of information included herein and shall have no liability for the consequences of use of such information. 4.2 Disclaimers Information in this document is provided solely to enable system and software implementers to use NXP products. There are no express or implied copyright licenses granted hereunder to design or fabricate any integrated circuits based on the information in this document. NXP reserves the right to make changes without further notice to any products herein. NXP makes no warranty, representation, or guarantee regarding the suitability of its products for any particular purpose, nor does NXP assume any liability arising out of the application or use of any product or circuit, and specifically disclaims any and all liability, including without limitation consequential or incidental damages. Typical parameters that may be provided in NXP data sheets and/ or specifications can and do vary in different applications, and actual performance may vary over time. All operating parameters, including typicals, must be validated for each customer application by customer's technical experts. NXP does not convey any license under its patent rights nor the rights of others. NXP sells products pursuant to standard terms and conditions of sale, which can be found at the following address: nxp.com/salestermsandconditions. 4.3 Trademarks Notice: All referenced brands, product names, service names and trademarks are property of their respective owners. NXP, the NXP logo, Freescale, and the Freescale logo are trademarks of NXP B.V. ARM and Cortex are registered trademarks of ARM Limited (or its subsidiaries) in the EU and/or elsewhere. All information provided in this document is subject to legal disclaimers. NXP B.V All rights reserved. Application note Rev June of 15

15 5. Contents 1. Introduction Terminology Mathematical Lemmas Lemma Lemma Lemma Kalman Filter Derivation Process Model Derivation Standard Kalman Equations Limiting Cases Legal information Definitions Disclaimers Trademarks Contents Please be aware that important notices concerning this document and the product(s) described herein, have been included in the section 'Legal information'. NXP B.V All rights reserved. For more information, visit: Date of release: 21 June 2016 Document identifier: