1、扩展卡尔曼滤波 随机非线性离散系统的数学描述 扩展卡尔曼滤波算法 计算 实例1. 随机非线性离散系统的数学描述随机非线性离散系统的信号模型和观测模型可描述为 1 ( , ) ( , ) k k k k k k x x xn ( , ) k k k kz H x w： M1的状态矢量： M1维的矢量: p1维的扰动噪声： Mp维 的矩阵( , )kk xkx ( , ) xkn ： N1的观测矢量： N1维的矢量： N1的 观测噪声矢量kz ( , )kkHxkw假定为不相关的白噪声， , kknw ( ) , ( ) T klE k E k l k n 0 n n Q( ) , ( ) T k
2、lk E k l k w 0 w w R( )TE k l n w 0系统的起始条件： 0 0 0 0 0 0 0( ) , ( ) ( ) x x x PTx x x xE k k E k k k k k 0 0 0 0 ( ) ) , ( ) )x n 0 x wTTxxE k k k E k k k 2. 扩展卡尔曼滤波算法非线性系统滤波的常用的方法是先将模型线性化，用线性化的模型近似，然后采用线性滤波方法。如果存在标称状态 （即无噪声时的状态解）， 则 可以在标称状态附近将非线性函数用泰勒级数展开、并取前两项使模型线性化。*kx如果无法确定标称状态，则可以将 在滤波值 附近 用泰勒级数
3、 展开，将 在预测值 附近 用泰勒级数 展开，并取前两项使模型线性化，由此建立的算法称为扩展的卡尔曼滤波 (EKF)算法( , )kk x / kkx( , )kkHx / 1 x / ( , ) ( , ) ( / , ) ( / ) k k kTkkk k k k k k k kk xx x x x x xx 高 阶 项 / ( , ) 1 ( / , ) ( / )( , ) k k kTkkk k k k k k kkk k k xx xx x x xx xn( / , )k k k x / ( , ) 1 ( / , ) ( / )( / , ) k k kTkkk k k k k
4、k kkk k k k xx xx x x xx / / ( , ) 1 ( , ) ( / , ) / ( / , ) k k kTk k kTkkkkkkkk k k k k k k k kk xxxx xxxx x xx xnx / ( , ) 1 ( / , ) ( / )( / , ) k k kTkkk k k k k k kkk k k k xx xx x x xx xn- +1 , kkku k 1 1 , k k k k k k k x xu n / ( , ) 1 , k k kTkkkkk xx xx / ( , ) ( / , ) / k k kTkkk k k k k
5、 kk xx xu xxx ( / , )k k k k x观测模型的线性化， / 1( , ) ( , ) ( / 1 , ) ( / 1 )k k kTkkk k k k k k k kk xxHxH x H x x xx高 阶 项- / 1( , ) k k kTkkkk xxHxHx / 1( , ) ( / 1 , ) / 1 k k kTkkk k k k k kk xxHxd H x xx k k k k k z H x d w预测： 1 / 1 , / k k k k k k k x xu扩展卡尔曼滤波算法： / ( , ) k k kTkkk xx xx / ( , ) (
6、/ , ) / k k kTkkk k k k kk xx x xxx 1 / ( / , )k k k k kx x滤波： / / 1 ( / 1 )k k k k k k k k k k x x K z H x d / 1( , ) k k kTkkk xxHxx / 1( , ) ( / 1 , ) / 1 k k kTkkk k k k kk xxHxH x xx / / 1 ( ( / 1 , ) )k k k k k k k k k x x K z H x扩展卡尔曼滤波算法小结：模型线性化 1 1 , k k k k k k k x xu n / ( , ) 1 , k k kTk
7、kkkk xx xx ( / , )k k k k x k k k k k z H x d w / 1( , ) k k kTkkkk xxHxHx预测：预测误差方差阵：滤波：滤波误差方差阵： / ( ) / 1 xxk k k k k k P I K H P增益： / 1 , 1 1 / 1 , 1 1 1 1 TTxxk k k k k k k k k k k P P Q 1 / 1 ( / 1 )TTxxk k k k k k k k k K P H H P H R / 1 ( 1 / 1 , 1 )k k k k k x x / / 1 ( ( / 1 , ) )k k k k k k k k k x x K z H x
