从最优估计角度理解卡尔曼滤波（Kalman Filter）
卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
若含噪声的系统观测模型如下：
X ( k + 1 ) = A X ( k ) + B u ( k ) + C w ( k ) Z ( k ) = H X ( k ) + v ( k ) \begin{array}{c} X(k+1)=A X(k)+B u(k)+C w(k) \\ Z(k)=H X(k)+v(k) \end{array} X(k+1)=AX(k)+Bu(k)+Cw(k)Z(k)=HX(k)+v(k)
其中 v , w v,w v,w服从多元高斯分布，二者相互独立，Q,R为噪声变量的协方差矩阵
p ( w ) ∼ N ( 0 , Q ) p ( v ) ∼ N ( 0 , R ) \begin{array}{l} p(w) \sim N(0, Q) \\ p(v) \sim N(0, R) \end{array} p(w)∼N(0,Q)p(v)∼N(0,R)
则根据反馈的思想，对状态量的估计值可表示为下式：
X ^ ( k ) = X ~ ( k ) + K ( k ) ( Z ( k ) − Z ~ ( k ) ) = X ~ ( k ) + K ( k ) ( Z ( k ) − H X ~ ( k ) ) \begin{aligned} \hat{X}(k)&=\tilde{X}(k)+K(k)(Z(k)-\tilde{Z}(k)) \\ &=\tilde{X}(k)+K(k)(Z(k)-H \tilde{X}(k)) \end{aligned} X^(k)=X~(k)+K(k)(Z(k)−Z~(k))=X~(k)+K(k)(Z(k)−HX~(k))
其中X ̃(k)为根据模型计算的预测值，Z(k)为测量值，Z ̃(k)为根据预测值计算的观测值的预测。
X ~ ( k ) = A X ^ ( k − 1 ) + B u ( k ) Z ~ ( k ) = H X ~ ( k ) \begin{array}{c} \tilde{X}(k)=A \hat{X}(k-1)+B u(k) \\ \tilde{Z}(k)=H \tilde{X}(k) \end{array} X~(k)=AX^(k−1)+Bu(k)Z~(k)=HX~(k)
Z ~ ( k ) − Z ~ ( k ) \tilde{Z}(k)-\tilde{Z}(k) Z~(k)−Z~(k)为实际观测输出值与预测值的误差,
那么目标是求得一个 K ( k ) K(k) K(k)使得 e ( k ) = X ( k ) − X ^ ( K ) e(k)=X(k)-\hat{X}(K) e(k)=X(k)−X^(K)的均方差最小，那么就意味着估计出的状态量与真实值最接近，也即估计最优。
真实值与估计值的误差的协方差矩阵为
P ( k ) = E [ e ( k ) e ( k ) ⊤ ] P(k)=E\left[e(k)e(k)^\top\right] P(k)=E[e(k)e(k)⊤]
同理，真实值与预测值误差的协方差矩阵为
P ′ ( k ) = E [ e ′ ( k ) e ′ ( k ) ⊤ ] e ′ ( k ) = X ( k ) − X ~ ( k ) {P^{'}}(k)=E\left[e^{'}(k)e^{'}(k)^\top\right]\\ e^{'}(k)=X(k)-\tilde{X}(k) P′(k)=E[e′(k)e′(k)⊤]e′(k)=X(k)−X~(k)
因为状态量与测量误差相互独立，所以上式可展开为
P ( k ) = P ′ ( k ) − K ( k ) H P ′ ( k ) − P ′ ( k ) H ⊤ K ( k ) ⊤ + K ( K ) ( H P ′ ( k ) H ⊤ + R ) K ( k ) ⊤ \begin{aligned} P(k)=&P^{'}(k)-K(k)HP^{'}(k)-P^{'}(k)H^{\top}K(k)^{\top}\\ &+K(K)(HP^{'}(k)H^{\top}+R)K(k)^{\top} \end{aligned} P(k)=P′(k)−K(k)HP′(k)−P′(k)H⊤K(k)⊤+K(K)(HP′(k)H⊤+R)K(k)⊤
且协方差矩阵的对角线元素即为各状态分量的方差，矩阵的迹即为误差e(k)的均方差。
那么求解
d tr ( P ( k ) ) d K ( k ) = 0 \frac{d\,\,\text{tr}(P(k))}{d\,\,K(k)}=0 dK(k)dtr(P(k))=0
即可得到满足需求的K(k)矩阵。
K ( k ) = P ′ ( k ) H ⊤ ( H P ′ ( k ) H ⊤ + R ) − 1 K(k)=P^{'}(k)H^{\top}(HP^{'}(k)H^{\top}+R)^{-1} K(k)=P′(k)H⊤(HP′(k)H⊤+R)−1
带入 P ( k ) P(k) P(k)可得
P ( k ) = ( I − K ( k ) H ) P ′ ( k ) P(k)=(I-K(k)H)P^{'}(k) P(k)=(I−K(k)H)P′(k)
其中,因为状态变量与噪声相互独立，可以写成
P ′ ( k + 1 ) = E [ e ′ ( k + 1 ) e ′ ( k + 1 ) T ] = E [ ( A ( x ( k ) − X ~ ( k ) ) + C w ( k ) ) ( A ( x ( k ) − X ~ ( k ) ) + C w ( k ) ) T ] = E [ ( A e ( k ) ) ( ( A e ( k ) ) T ] + E [ C w ( k ) w ( k ) T C T ] = A P ( k ) A T + C Q C T \begin{aligned} P^{\prime}(k+1)&=E\left[e^{\prime}(k+1) e^{\prime}(k+1)^{T}\right]\\ &=E\left[(A(x(k)-\tilde{X}(k))+C w(k))(A(x(k)-\tilde{X}(k))+C w(k))^{T}\right]\\ &=E\left[(A e(k))\left((A e(k))^{T}\right]+E\left[C w(k) w(k)^{T} C^{T}\right]\right.\\ &=A P(k) A^{T}+C Q C^{T} \end{aligned} P′(k+1)=E[e′(k+1)e′(k+1)T]=E[(A(x(k)−X~(k))+Cw(k))(A(x(k)−X~(k))+Cw(k))T]=E[(Ae(k))((Ae(k))T]+E[Cw(k)w(k)TCT]=AP(k)AT+CQCT
综上，可得卡尔曼滤波公式
X ^ ( k ) = X ~ ( k ) + K ( k ) ( Z ( k ) − H X ~ ( k ) ) X ~ ( k ) = A X ^ ( k − 1 ) + B u ( k ) K ( k ) = P ′ ( k ) H T ( H P ′ ( k ) H T + R ) − 1 P ′ ( k + 1 ) = A P ( k ) A T + C Q C T P ( k ) = ( I − K ( k ) H ) P ′ ( k ) \begin{aligned} \hat{X}(k)&=\tilde{X}(k)+K(k)(Z(k)-H \tilde{X}(k))\\ \tilde{X}(k)&=A \hat{X}(k-1)+B u(k) \\ K(k)&=P^{\prime}(k) H^{T}\left(H P^{\prime}(k) H^{T}+R\right)^{-1}\\ P^{\prime}(k+1)&=A P(k) A^{T}+C Q C^{T}\\ P(k)&=(I-K(k) H) P^{\prime}(k) \end{aligned} X^(k)X~(k)K(k)P′(k+1)P(k)=X~(k)+K(k)(Z(k)−HX~(k))=AX^(k−1)+Bu(k)=P′(k)HT(HP′(k)HT+R)−1=AP(k)AT+CQCT=(I−K(k)H)P′(k)
Q Q Q表示对模型的信任程度
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
