卡尔曼滤波器(思路整理,算法推导)
知乎好文:卡尔曼滤波
- 首先定义好各个变量:
状态的最优估计值: x ^ t − 1 , x ^ t \hat{x}_{t-1}, \hat{x}_{t} x^t−1,x^t
状态的预测值: x ^ t − \hat{x}^{-}_{t} x^t−
状态的真实值: x t − 1 , x t {x}_{t-1}, x_{t} xt−1,xt
状态的观测值: z t z_{t} zt
状态转移矩阵: F F F
控制输入矩阵: B B B
控制变量: u t u_{t} ut
卡尔曼增益: K K K
状态观测矩阵: H H H
状态转移过程噪声: w t w_t wt遵循高斯分布 ( 0 , Q ) (0,Q) (0,Q)
状态观测噪声: v t v_t vt遵循高斯分布 ( 0 , R ) (0,R) (0,R)
- 先解释下卡尔曼滤波器完成的是一件什么事
默认:状态的真实值是无法直接得到的,只能尽可能估计。
最优估计:根据上一次状态的最优估计值
x
^
t
−
1
\hat{x}_{t-1}
x^t−1得到当前状态的预测值
x
^
t
−
\hat{x}^{-}_{t}
x^t−,由于预测不一定准确,因此需要根据观察值加以修正,也就是说最优估计值是预测值与观测值的加权和,加权和即为滤波,此即卡尔曼滤波器:
x
^
t
=
x
^
t
−
+
K
(
z
t
−
H
x
^
t
−
)
\hat{x}_{t} = \hat{x}^{-}_{t}+K(z_t-H\hat{x}^{-}_{t})
x^t=x^t−+K(zt−Hx^t−)
以一维为例,一般而言,H为1,因此K等于1时,状态最优估计值完全等于观测值,K等于0时完全等于预测值。
- 公式推导
x ^ t − = F x ^ t − 1 + B u t \hat{x}^{-}_{t} = F\hat{x}_{t-1}+Bu_t x^t−=Fx^t−1+But
x t = F x t − 1 + B u t + w t x_t = Fx_{t-1}+Bu_t+w_t xt=Fxt−1+But+wt
因此真实值与预测值之差的方差为
P
t
−
=
c
o
v
(
x
t
−
x
^
t
−
)
=
c
o
v
(
F
(
x
t
−
1
−
x
^
t
−
1
)
+
w
t
)
=
F
c
o
v
(
x
t
−
1
−
x
^
t
−
1
)
F
T
+
Q
=
F
P
t
−
1
F
T
+
Q
P^{-}_{t}=cov(x_t-\hat{x}^{-}_{t})=cov(F(x_{t-1}-\hat{x}_{t-1})+w_t)=\\Fcov(x_{t-1}-\hat{x}_{t-1})F^T+Q=FP_{t-1}F^T+Q
Pt−=cov(xt−x^t−)=cov(F(xt−1−x^t−1)+wt)=Fcov(xt−1−x^t−1)FT+Q=FPt−1FT+Q
z t = H x t + v t z_t=Hx_{t}+v_t zt=Hxt+vt
卡尔曼滤波器的目的:
m
i
n
∣
∣
x
^
t
−
x
t
∣
∣
2
min||\hat{x}_{t}-x_t||^2
min∣∣x^t−xt∣∣2
x
^
t
=
x
^
t
−
+
K
(
H
x
t
+
v
t
−
H
x
^
t
−
)
x
^
t
−
x
t
=
(
I
−
K
H
)
(
x
^
t
−
−
x
t
)
+
K
v
t
c
o
v
(
x
^
t
−
x
t
)
=
c
o
v
(
(
I
−
K
H
)
(
x
^
t
−
−
x
t
)
+
K
v
t
)
P
t
=
(
I
−
K
H
)
P
t
−
(
I
−
K
H
)
T
+
K
R
K
T
\hat{x}_{t} =\hat{x}^{-}_{t}+K(Hx_{t}+v_t-H\hat{x}^{-}_{t}) \\ \hat{x}_{t}-x_t=(I-KH)(\hat{x}^{-}_{t}-x_t)+Kv_t \\ cov(\hat{x}_{t}-x_t) = cov((I-KH)(\hat{x}^{-}_{t}-x_t)+Kv_t) \\ P_t = (I-KH)P^{-}_{t}(I-KH)^T+KRK^T
x^t=x^t−+K(Hxt+vt−Hx^t−)x^t−xt=(I−KH)(x^t−−xt)+Kvtcov(x^t−xt)=cov((I−KH)(x^t−−xt)+Kvt)Pt=(I−KH)Pt−(I−KH)T+KRKT
要满足
m
i
n
∣
∣
x
^
t
−
x
t
∣
∣
2
min||\hat{x}_{t}-x_t||^2
min∣∣x^t−xt∣∣2,需要
m
i
n
T
r
(
P
t
)
minTr(P_t)
minTr(Pt),注意这里假设
x
x
x为多变量,因此各个维度差值的方差和应尽可能小
接下来一步不太理解:
P
t
P_t
Pt对
K
K
K求偏导应为0(这样就能保证迹最小?望大佬告知):
∂
(
P
t
)
/
∂
(
K
)
=
−
2
(
P
t
−
H
T
)
+
2
K
(
H
P
t
−
H
T
+
R
)
=
0
K
=
(
P
t
−
H
T
)
(
H
P
t
−
H
T
+
R
)
−
1
\partial(P_t)/\partial(K) = -2(P^{-}_{t}H^T)+2K(HP_t^-H^T+R)=0 \\ K = (P^{-}_{t}H^T)(HP_t^-H^T+R)^{-1}
∂(Pt)/∂(K)=−2(Pt−HT)+2K(HPt−HT+R)=0K=(Pt−HT)(HPt−HT+R)−1
由此可以得到
P
t
=
(
I
−
K
H
)
P
t
−
P_t=(I-KH)P_t^-
Pt=(I−KH)Pt−
因此卡尔曼滤波的整个过程为:
- 根据 x ^ t − 1 \hat{x}_{t-1} x^t−1得到预测值 x ^ t − \hat{x}^-_t x^t−,再通过观测得到观测值 z t z_t zt
- 根据上一时刻的方差 P t − 1 P_{t-1} Pt−1得到 P t − P^-_{t} Pt−,再根据 H , R H,R H,R求得最佳卡尔曼增益 K K K
- 根据 x ^ t − , z t , H , K \hat{x}^-_t,z_t,H,K x^t−,zt,H,K得到最优估计值 x ^ t \hat{x}_{t} x^t
- 最后更新下一步需要用到的 P t P_t Pt
总结如下:
图中红色为输入,蓝色为输出,浅绿色与深绿色部分对应预测过程,白色、黄色、橙色代表更新过程。
Tips:习惯 x ^ 0 = 0 , P 0 = 1 \hat{x}_0=0,P_0=1 x^0=0,P0=1, Q , R Q,R Q,R是可以调节的超参数