1. 概述
卡尔曼滤波（Kalman Filter, KF）是一种线性、无偏和估计方差最小的最优估计算法。KF本质是估计+测量同时考虑的迭代优化过程。其适用于线性高斯系统。
卡尔曼滤波器的优势：不需要追溯很久之前的数据，只需要上一次的就可以。
状态方程：
x
k
=
A
x
k
−
1
+
B
u
k
+
w
k
x_{k}=Ax_{k-1}+Bu_{k}+w_{k}
xk=Axk−1+Buk+wk其中，
A
A
A为状态转移矩阵，
B
B
B为输入状态矩阵，
w
k
w_{k}
wk为过程噪声（高斯噪声）。
测量方程：
y
k
=
C
x
k
+
v
k
y_k=Cx_k+v_k
yk=Cxk+vk其中，
y
k
y_k
yk为测量值，
C
C
C为测量转换矩阵，
v
k
v_k
vk为测量噪声（传感器测量误差）。
w
k
w_{k}
wk和
v
k
v_k
vk均符合高斯分布：
ω
k
∈
N
(
0
;
Q
k
)
v
k
∈
N
(
0
;
R
k
)
\begin{array}{l} \omega_{k} \in N\left(0 ; Q_{k}\right) \\ v_{k} \in N\left(0 ; R_{k}\right) \end{array}
ωk∈N(0;Qk)vk∈N(0;Rk)
2. 卡尔曼公式理解
实现过程：使用上一次的最优结果预测当前的值，同时使用测量值修正当前值，得到最优结果。
步骤：
- 先验估计
x ^ t − = F x ^ t − 1 + B u t − 1 + w t \hat{x}_{t}^{-}=F \hat{x}_{t-1}+B u_{t-1}+w_{t} x^t−=Fx^t−1+But−1+wt - 先验估计协方差
P t − = cov ( x ^ t − , x ^ t − ) = cov ( F x ^ t − 1 + B u t − 1 + w t , F x ^ t − 1 + B u t − 1 + w t ) = F cov ( x ^ t − 1 , x ^ t − 1 ) F T + Q = F P t − 1 F T + Q \begin{aligned} P_{t}^{-}&=\operatorname{cov}\left(\hat{x}_{t}^{-}, \hat{x}_{t^{-}}\right)\\ &=\operatorname{cov}\left(F \hat{x}_{t-1}+B u_{t-1}+w_{t}, F \hat{x}_{t-1}+B u_{t-1}+w_{t}\right) \\ &=F \operatorname{cov}\left(\hat{x}_{t-1}, \hat{x}_{t-1}\right) F^{T}+Q\\ &=F P_{t-1} F^{T}+Q \end{aligned} Pt−=cov(x^t−,x^t−)=cov(Fx^t−1+But−1+wt,Fx^t−1+But−1+wt)=Fcov(x^t−1,x^t−1)FT+Q=FPt−1FT+Q注：此时 u t − 1 u_{t-1} ut−1相当于一个常数，故其协方差为0 - 测量方程
z t = H x t + v z_{t}=H x_{t}+v zt=Hxt+v - 修正估计（最终结果）
x ^ t = x ^ t − + K t ( z t − H x ^ t − ) \hat{x}_{t}=\widehat{x}_{t}^{-}+K_{t}\left(z_{t}-H \hat{x}_{t}^{-}\right) x^t=x t−+Kt(zt−Hx^t−) - 更新卡尔曼增益
K t = P t − H T H P t − H T + R K_{t}=\frac{P_{t}^{-} H^{T}}{H P_{t}^{-} H^{T}+R} Kt=HPt−HT+RPt−HT - 更新后验估计协方差
P t = ( I − K t H ) P t − P_{t}=\left(I-K_{t} H\right) P_{t}^{-} Pt=(I−KtH)Pt−
调节Q与R的取值：
由1和5可得，在H取1时，有：
K t = P t − 1 + Q P t − 1 + Q + R K_{t}=\frac{P_{t-1}+Q}{P_{t-1}+Q+R} Kt=Pt−1+Q+RPt−1+Q则代入4，可得根据调节Q和R即可调整权重使得结果更好。
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
