三种卡尔曼滤波理论

1、精选优质文档-倾情为你奉上经典卡尔曼滤波动态系统的卡尔曼滤波数学模型包括状态方程和观测方程，对于线性系统，其离散形式为Xk= k,k-1Xk-1+ Gk-1Wk-1Lk= HkXk+ Vk其中，Xk为系统在tk时刻的n×1维状态向量，k,k-1为系统从tk-1时刻到tk时刻的n×n维状态转移矩阵，Wk-1为tk-1时刻的r×1动态噪声，Gk-1为tk-1时刻的n×r动态噪声矩阵，Lk为系统在tk时刻的m×1维观测向量，Hk为系统在tk时刻的m×n维观测矩阵，Vk为系统在tk时刻的m×1维观测噪声。根据最小二乘原则，可推得卡尔

2、曼滤波递推公式为：1）状态向量一步预测值为 Xk,k-1=k,k-1 Xk-1,k-12）状态向量一步预测值方差矩阵为Pk,k-1= k,k-1 Pk-1,k-1k,k-1T+Gk-1Qk-1Gk-1T 其中，Qk为动态噪声方差矩阵。3）状态向量估计值为Xk,k= Xk,k-1+JkLk- HkXk,k-14）状态向量估计值方差矩阵为Pk,k= I- JkHkPk,k-1其中，Jk为滤波增益矩阵，具体形式如下Jk= Pk,k-1HkTHkPk,k-1HkT+ Rk-1Rk为观测噪声方差矩阵。基于方差补偿的自适应卡尔曼滤波假定Wk和Vk是正态序列，X0是正态向量。则定义l步预测残差是Vk+l=

3、Lk+l- Lk+l,k其中， Lk+l，Lk+l,k分别为第k+l期观测值和它的最佳观测值Lk+l,k= Hk+lk+l,kXk,k，则Vk+l为正态向量。Vk+l的方差阵Svv为：Svv= Hk+lk+l,kPkk+l,kTHk+lT+Rk+l+ i=1lHk+lk+l,k+iGk+l,k+i-1Qk+i-1Gk+l,k+i-1Tk+l,k+iTHk+lT记Hk+lk+l,k+iGk+l,k+i-1= Ak+l,k+i= ahjk+l,k+i，式中h=1,2,m；j=1,2,r。假定Qk+i-1在观测时间段tk+1，tk+2，tk+N上为常值对角阵，即Qk+i-1= 11200rr2=Q并

4、记diagQ= 112,222,rr2T。根据EVk+lTVk+l=trVk+lVk+lT=trSvv，于是记Vk+lTVk+l=trSvv+ k+l其中，k+l为零均值随机变量，l=1,2,N。令Ek+l= Vk+lTVk+l-trHk+lk+l,kPkk+l,kTHk+lT-trRk+l又记E= Ek+l,Ek+NT= k+l,k+NTA= Ak+l,Ak+NT则有E=AdiagQ+ 上式就是关于diagQ的线性方程组。当Nr时，有唯一解。记diagQ的LS估计为diagQ= ATA-1ATE根据上面各式求得任意长度时间段上的Q，把它作为动态噪声协方差阵的实时估计。基于极大验后估计的自适应

5、卡尔曼滤波动态噪声向量Wk、观测噪声向量Vk是互相独立的正态白噪声向量，满足EWk=q,EVk=r，若噪声的统计特性已知，可由极大验后估计原理得到自适应卡尔曼滤波的递推方程为：1）状态向量一步预测值为 Xk,k-1=k,k-1 Xk-1,k-1+ qk-12）状态向量一步预测值方差矩阵为Pk,k-1= k,k-1 Pk-1,k-1k,k-1T+Qk-1 3）状态向量估计值为Xk,k= Xk,k-1+JkLk- HkXk,k-14）状态向量估计值方差矩阵为Pk,k= I- JkHkPk,k-1其中，Jk为滤波增益矩阵，具体形式如下Jk= Pk,k-1HkTHkPk,k-1HkT+ Rk-1Rk为观测噪声方差矩阵。若噪声均值向量q,r和协方差阵Qk，Rk未知时，可以利用滤波估值Xj,j和预报值Xj,j-1近似代替计算较复杂的平滑估值Xk,j和Xk,j-1。由于地铁站口基坑的开挖施工，对