卡尔曼滤波器原理实现.zip

卡尔曼滤波器原理实现： X(k|k-1) = AX(k-1|k-1) + BU(k) P(k|k-1) = AP(k-1|k-1)A.T + Q K(k) = P(k|k-1)H.T / (HP(k|k-1)H.T + R) X(k|k) = X(k|k-1) + K(k)[Z(k) - HX(k|k-1)] P(k|k) = [1 - K(k)H]P(k|k-1) 第1步：计算中间量X(k|k-1)、P(k|k-1)。 第2步：计算卡尔曼增益K(k)。 第3步：计算当前时刻的状态最佳估计：X(k|k)、P(k|k)。