离散时间下的卡尔曼滤波器

  已知运动方程为， x k = ϕ k ∣ k − 1 x k − 1 + Γ k − 1 w k − 1 (1) x_k=phi_{k|k-1}x_{k-1}+Gamma_{k-1}w_{k-1} ag{1} xk=ϕk∣k−1xk−1+Γk−1wk−1(1) 其中 x k x_k xk表示 k k k时刻的状态， ϕ k ∣ k − 1 phi_{k|k-1} ϕk∣k−1表示状态转移矩阵， x k − 1 x_{k-1} xk−1表示 k − 1 k-1 k−1时刻的状态， Γ k − 1 Gamma_{k-1} Γk−1表示噪声矩阵， w k − 1 w_{k-1} wk−1表示运动噪声。 w k − 1 w_{k-1} wk−1服从高斯分布 N ( 0 , Q k − 1 ) N(0,Q_{k-1}) N(0,Qk−1)。   测量方程为， z k = H k x k + v k (2) z_k=H_kx_k+v_k ag{2} zk=Hkxk+vk(2) 其中 z k z_k zk表示 k k k时刻的测量， H k H_k Hk表示测量矩阵， v k v_k vk表示测量噪声。 v k v_k vk服从高斯分布 N ( 0 , R k ) N(0,R_k) N(0,Rk)。   输入 k − 1 k-1 k−1时刻的后验状态 x ^ k − 1 hat x_{k-1} x^k−1和后验状态的协方差矩阵 P ^ k − 1 hat P_{k-1} P^k−1，以及 k k k时刻的测量 z k z_k zk，进行如下操作：预测+更新。   预测：用运动方程得到先验估计，包括先验状态 x ˇ k check x_k xˇk和先验状态的协方差矩阵 P ˇ k check P_k Pˇk。 x ˇ k = ϕ k ∣ k − 1 x ^ k − 1 (3-1) check x_k = phi_{k|k-1}hat x_{k-1} ag{3-1} xˇk=ϕk∣k−1x^k−1(3-1) P ˇ k = ϕ k ∣ k − 1 P ^ k − 1 ϕ k ∣ k − 1 T + Γ k − 1 Q k − 1 Γ k − 1 T (3-2) check P_k=phi_{k|k-1}hat P_{k-1}phi_{k|k-1}^T+Gamma_{k-1}Q_{k-1}Gamma_{k-1}^T ag{3-2} Pˇk=ϕk∣k−1P^k−1ϕk∣k−1T+Γk−1Qk−1Γk−1T(3-2) 其中公式(3-1)由均值的传播定律得到，而公式(3-2)由协方差的传播定律得到。   更新：先计算卡尔曼增益，然后用测量方程更新先验估计得到后验估计，包括后验状态 x ^ k hat x_k x^k和后验状态的协方差矩阵 P ^ k hat P_k P^k。 K k = P ˇ k H k T ( H k P ˇ k H k T + R k ) − 1 (3-3) K_k=check P_kH_k^T(H_kcheck P_kH_k^T+R_k)^{-1} ag{3-3} Kk=PˇkHkT(HkPˇkHkT+Rk)−1(3-3)

x ^ k = x ˇ k + K k ( z k − H k x ˇ k ) (3-4) hat x_k = check x_k + K_k(z_k-H_kcheck x_k) ag{3-4} x^k=xˇk+Kk(zk−Hkxˇk)(3-4)

P ^ k = ( I − K k H k ) P ˇ k (3-5) hat P_k= (I-K_kH_k)check P_k ag{3-5} P^k=(I−KkHk)Pˇk(3-5) 其中公式(3-1)至(3-5)即为离散时间下的卡尔曼滤波器方程。

  卡尔曼滤波器的一个小例子。   已知房间内的温度受扰动噪声 w w w的影响，它服从高斯分布 N ( 0 , 0. 4 2 ) N(0,0.4^2) N(0,0.42)；同时，房间内有一个温度计，它的测量噪声为 v v v，它服从高斯分布 N ( 0 , 0. 3 2 ) N(0,0.3^2) N(0,0.32)。已知 k − 1 k-1 k−1时刻房间内的真实温度为25摄氏度， k k k时刻的温度计读数为25.2摄氏度，请问 k k k时刻房间的温度估计为多少？该估计的不确定度为多少？   解答： 由题意，系统的运动方程为， x k = x k − 1 + w k − 1 x_k=x_{k-1}+w_{k-1} xk=xk−1+wk−1 测量方程为， z k = x k + v k z_k=x_k+v_k zk=xk+vk 且已知 k − 1 k-1 k−1时刻的后验估计为25摄氏度，方差为0。根据公式(3-1)和(3-2)得到， k k k时刻先验的状态估计为25摄氏度，方差为 0. 4 2 0.4^2 0.42。根据公式(3-3)得到卡尔曼增益为， K k = 0. 4 2 0. 4 2 + 0. 3 2 = 16 25 = 0.64 K_k=frac{0.4^2}{0.4^2+0.3^2}=frac{16}{25}=0.64 Kk=0.42+0.320.42=2516=0.64 那么，根据公式(3-4)和公式(3-5)得到后验的状态估计为， x ^ k = 25 + 0.64 ⋅ ( 25.2 − 25 ) = 25.128 hat x_k=25+0.64cdot(25.2-25)=25.128 x^k=25+0.64⋅(25.2−25)=25.128 P ^ k = ( 1 − 0.64 ) ⋅ 0. 4 2 = 0.0576 = 0.2 4 2 hat P_k=(1-0.64)cdot0.4^2=0.0576=0.24^2 P^k=(1−0.64)⋅0.42=0.0576=0.242 故 k k k时刻房间的温度估计为25.128摄氏度，该估计的不确定度为 0.2 4 2 0.24^2 0.242。   换个角度思考（当运动方程和测量方程中的系数皆为1时，下面两个结论才成立）， 预测操作时，状态的不确定度等价于电阻的串联， P ˇ k = 0 2 + 0. 4 2 ⇒ P ˇ k = 0. 4 2 check P_k=0^2+0.4^2 Rightarrow check P_k=0.4^2 Pˇk=02+0.42⇒Pˇk=0.42 更新操作时，状态的不确定度等价于电阻的并联， 1 P ^ k = 1 0. 4 2 + 1 0. 3 2 ⇒ P ^ k = 0.2 4 2 frac{1}{hat P_k}=frac{1}{0.4^2}+frac{1}{0.3^2} Rightarrow hat P_k=0.24^2 P^k1=0.421+0.321⇒P^k=0.242