一维卡尔曼滤波器仿真

1.离散线性卡尔曼滤波器推导

（1）状态传递
    X(k) = AX(k-1) + BU(k) + v
   A为传递矩阵，B为输入控制矩阵， v为均值为0,方差为Q的系统噪声，X为估计状态，X(k)协方差为P(k)，则P(k) = A*P(k-1)A’ + Q
（2）系统观测
     Z(K) = z(k)+w
   w为均值为0,方差为R的观测误差，z(k)为真值
（3）由预测和观测得到最优估计
    由状态估计到输出估计：Z’(k) = HX(k)，H为观测矩阵，将状态转变为输出，Z’(k)服从(H(k)*X(k), HP(k)H’)的正太分布，观测值服从(z(k), R)的正太分布。最优估计分布由预测分布和观测分布相乘得到，两个正太分布的乘积仍为正太分布，新分布的均值和方差为：
           
           
设最优状态估计X_pre, 协方差P_pre
HX_pre(k) = HX(k)+k(z(k) - HX(k))
HP_pre(k)H’=HP(k)H’-kHP(k)H’
k = HP(k)H’(HP(k)H’+R)^-1
上面第一个等式左乘H^-1， 第二个等式左乘H^-1， 右乘H’^-1，得到：
X_pre(k) = X(k) + k’(z(k)-HX(k))
P_pre(k) = P(k) - k’HP(k)
k’ = P(k)H’(HP(k)H’+R)^-1
整个过程可由下图表示：

2.一维离散卡尔曼滤波matlab仿真

结果：