前言
名声在外的Kalman Filter不用说多, Dr_Can的教程是最好入门的, 另外前面总结过一些基础的矩阵分解,Gaussian操作后,KF也可以用得上,下面总结一下KF的推导过程. (Gaussian 的MVN以及相关操作可见链接)
正文
状态系统方程
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}
xk=Axk−1+Buk−1+wk−1
量测模型方程
z
k
=
H
x
k
+
v
k
z_k = Hx_k+v_k
zk=Hxk+vk
w
k
∼
N
(
0
,
Q
)
w_k\sim \mathcal{N}(0,Q)
wk∼N(0,Q) 过程噪声,
v
k
∼
N
(
0
,
R
)
v_k\sim \mathcal{N}(0,R)
vk∼N(0,R) 测量噪声.
设有:
x
^
k
−
\hat{x}^{-}_k
x^k−为先验状态估计,由过程模型的上一步推算可知;
x
^
k
\hat{x}_k
x^k为后验状态估计由当前这一步的测量可知;
x
k
x_k
xk为真值.
e
k
−
=
x
k
−
x
^
k
−
e
k
=
x
k
−
x
^
k
\begin{split} e^{-}_k &= x_k-\hat{x}^{-}_k\\ e_k &= x_k - \hat{x}_k \end{split}
ek−ek=xk−x^k−=xk−x^k
e
k
−
e^{-}_k
ek−是先验误差,
e
k
e_k
ek为后验误差.
P k − = E [ e k − ⊤ e k − ] P^{-}_k=E[{e^{-}_k}^{\top}e^{-}_k] Pk−=E[ek−⊤ek−] 先验估计误差的协方差; P k = E [ e k ⊤ e k ] P_k=E[{e_k}^{\top}e_k] Pk=E[ek⊤ek] 后验估计误差的协方差;
x
^
k
\hat{x}_k
x^k这一后验估计被视为先验估计
x
^
k
−
\hat{x}^{-}_k
x^k−以及实际测量
z
k
z_k
zk与测量预测
H
x
^
k
−
H\hat{x}^{-}_k
Hx^k−的加权和
x
^
=
x
^
−
+
K
(
z
k
−
H
x
^
−
)
\hat{x} = \hat{x}^{-}+K(z_k - H\hat{x}^{-})
x^=x^−+K(zk−Hx^−)
z k − H x ^ k − z_k-H\hat{x}^{-}_k zk−Hx^k−被视为测量残差. 表现了实际测量与测量模型预测值之间的差别.
x
^
k
=
x
^
k
−
+
K
(
H
∗
x
k
+
v
k
−
H
x
^
k
−
)
x
^
k
−
x
k
=
x
^
k
−
−
x
k
+
K
H
(
x
k
−
x
^
k
−
)
+
K
v
k
\begin{split} \hat{x}_k &= \hat{x}^{-}_k+K(H*x_k+v_k-H\hat{x}^{-}_k)\\ \hat{x}_k-x_k &= \hat{x}^{-}_k-x_k+KH(x_k-\hat{x}^{-}_k)+Kv_k \end{split}
x^kx^k−xk=x^k−+K(H∗xk+vk−Hx^k−)=x^k−−xk+KH(xk−x^k−)+Kvk
e
k
=
e
k
−
−
K
H
(
e
k
−
)
−
K
v
k
e
k
=
(
I
−
K
H
)
e
k
−
−
K
v
k
\begin{split} e_k &= e^{-}_k-KH(e^{-}_k)-Kv_k\\ e_k &= (I-KH)e^{-}_k-Kv_k \end{split}
ekek=ek−−KH(ek−)−Kvk=(I−KH)ek−−Kvk
写出
P
k
P_k
Pk(后验估计误差的协方差矩阵)有(Gaussian 操作的MVN):
P
k
=
E
[
e
k
⊤
e
k
]
=
(
I
−
K
H
)
P
k
−
(
I
−
K
H
)
⊤
+
K
R
K
⊤
P_k = E[e^{\top}_ke_k] = (I-KH)P^{-}_k(I-KH)^{\top}+KRK^{\top}
Pk=E[ek⊤ek]=(I−KH)Pk−(I−KH)⊤+KRK⊤
(为什么是和?误差与噪声相互独立)
P
k
=
P
k
−
−
K
H
P
k
−
−
P
k
−
H
⊤
K
⊤
+
K
H
P
k
−
H
⊤
K
⊤
+
K
R
K
⊤
P
k
=
P
k
−
−
K
H
P
k
−
−
P
k
−
H
⊤
K
⊤
+
K
(
H
P
k
−
H
⊤
+
K
R
)
K
⊤
\begin{split} P_k &= P^{-}_k-KHP^{-}_k-P^{-}_kH^{\top}K^{\top}+KHP^{-}_kH^{\top}K^{\top}+KRK^{\top}\\ P_k &= P^{-}_k-KHP^{-}_k-P^{-}_kH^{\top}K^{\top}+K(HP^{-}_kH^{\top}+KR)K^{\top} \end{split}
PkPk=Pk−−KHPk−−Pk−H⊤K⊤+KHPk−H⊤K⊤+KRK⊤=Pk−−KHPk−−Pk−H⊤K⊤+K(HPk−H⊤+KR)K⊤
KF的损失函数: 协方差
P
k
P_k
Pk最小, (协方差越小, 误差越集中, 假如为单位阵,直接补偿就可以得到一个精确的模型)
J
=
∑
m
i
n
P
k
J = \sum_{min}P_k
J=min∑Pk
P
k
P_k
Pk的迹(为什么是trace?)对K求偏导有:
不过要先注意有:
(
(
P
k
−
H
⊤
)
K
⊤
)
⊤
=
K
(
P
k
−
H
⊤
)
⊤
=
K
H
P
k
−
⊤
((P^{-}_kH^{\top})K^{\top})^{\top}=K(P^{-}_kH^{\top})^{\top}=KH{P^{-}_k}^{\top}
((Pk−H⊤)K⊤)⊤=K(Pk−H⊤)⊤=KHPk−⊤
对于方阵有:
t
r
(
A
)
=
t
r
(
A
T
)
tr(A)=tr(A^T)
tr(A)=tr(AT)
∂
P
k
∂
K
=
∂
t
r
(
P
k
−
)
∂
K
−
∂
2
t
r
(
K
H
P
k
−
)
∂
K
+
∂
t
r
(
K
H
P
k
−
H
⊤
K
⊤
)
∂
K
+
∂
t
r
(
K
R
K
⊤
)
∂
K
\frac{\partial P_k}{\partial K} = \frac{\partial tr(P^{-}_k)}{\partial K}-\frac{\partial 2tr(KHP^{-}_k)}{\partial K} +\frac{\partial tr(KHP^{-}_kH^{\top}K^{\top})}{\partial K}+\frac{\partial tr(KRK^{\top})}{\partial K}
∂K∂Pk=∂K∂tr(Pk−)−∂K∂2tr(KHPk−)+∂K∂tr(KHPk−H⊤K⊤)+∂K∂tr(KRK⊤)
有:
∂
t
r
(
A
B
)
∂
A
=
B
⊤
\frac{\partial tr(AB)}{\partial A}=B^{\top}
∂A∂tr(AB)=B⊤,
∂
A
B
A
⊤
∂
A
=
2
A
B
\frac{\partial ABA^{\top}}{\partial A} =2AB
∂A∂ABA⊤=2AB
∂
P
k
∂
K
=
0
−
2
P
k
−
⊤
H
⊤
+
2
K
H
P
k
−
H
⊤
+
2
K
R
\frac{\partial P_k}{\partial K} = 0-2{P^{-}_k}^{\top}H^{\top}+2KHP^{-}_kH^{\top}+2KR
∂K∂Pk=0−2Pk−⊤H⊤+2KHPk−H⊤+2KR
令其等于0,变换出K有
P
k
−
=
P
k
−
⊤
P^{-}_k={P^{-}_k}^{\top}
Pk−=Pk−⊤, 协方差矩阵对称
K
=
P
k
−
H
⊤
H
P
k
−
H
⊤
+
R
K= \frac{P^{-}_kH^{\top}}{HP^{-}_kH^{\top}+R}
K=HPk−H⊤+RPk−H⊤
该K为Kalman Gain.
再次回到状态系统方程与测量模型中
x
k
=
A
x
k
+
1
+
B
u
k
−
1
+
w
k
−
1
x_k = Ax_{k+1}+Bu_{k-1}+w_{k-1}
xk=Axk+1+Buk−1+wk−1
z
k
=
H
x
k
+
v
k
z_k = Hx_k+v_k
zk=Hxk+vk
K
=
P
k
−
H
⊤
H
P
k
−
H
⊤
+
R
K = \frac{P^{-}_kH^{\top}}{HP^{-}_kH^{\top}+R}
K=HPk−H⊤+RPk−H⊤
要知道K, 还需要求出
P
k
−
P^{-}_k
Pk−(先验估计误差协方差矩阵,不要与
P
k
P_k
Pk搞混乱了)
e
k
−
=
x
k
−
x
^
k
−
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
−
A
x
^
k
−
1
−
B
u
k
−
1
=
A
(
x
k
−
1
−
x
^
k
−
1
)
+
w
k
−
1
=
A
e
k
−
1
+
w
k
−
1
e^{-}_k=x_k-\hat{x}^{-}_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}-A\hat{x}_{k-1}-Bu_{k-1}=A(x_{k-1}-\hat{x}_{k-1})+w_{k-1}=Ae_{k-1}+w_{k-1}
ek−=xk−x^k−=Axk−1+Buk−1+wk−1−Ax^k−1−Buk−1=A(xk−1−x^k−1)+wk−1=Aek−1+wk−1
x
^
k
−
\hat{x}^{-}_k
x^k−为第k步的先验状态估计(直接由模型计算得出).
P
k
−
=
E
[
e
k
−
e
k
−
⊤
]
=
E
[
(
A
e
k
−
1
+
w
k
−
1
)
(
A
e
k
−
1
+
w
k
−
1
)
⊤
]
=
E
[
A
e
k
−
1
e
k
−
1
⊤
A
⊤
+
A
e
k
−
1
w
k
−
1
⊤
+
w
k
−
1
e
k
−
1
⊤
A
⊤
+
w
k
−
1
⊤
w
k
−
1
]
=
E
[
A
e
k
−
1
e
k
−
1
⊤
A
⊤
]
+
E
[
A
e
k
−
1
w
k
−
1
⊤
]
+
E
[
w
k
−
1
e
k
−
1
⊤
A
⊤
]
+
E
[
w
k
−
1
⊤
w
k
−
1
]
e
k
−
1
=
x
k
−
1
−
x
^
k
−
1
真值减去后验与
w
k
−
1
相互独立
=
E
[
A
e
k
−
1
e
k
−
1
⊤
A
⊤
]
+
E
[
w
k
−
1
w
k
−
1
⊤
]
第二项为过程噪声的协方差
Q
=
A
P
k
−
1
A
⊤
+
Q
\begin{split} P^{-}_k &= E[{e^{-}_k}{e^{-}_k}^{\top}]\\ &= E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^{\top}]\\ &= E[Ae_{k-1}{e_{k-1}}^{\top}A^{\top}+Ae_{k-1}w^{\top}_{k-1}+w_{k-1}e^{\top}_{k-1}A^{\top}+{w_{k-1}}^{\top}w_{k-1}]\\ &= E[Ae_{k-1}{e_{k-1}}^{\top}A^{\top}]+E[Ae_{k-1}w^{\top}_{k-1}]+E[w_{k-1}e^{\top}_{k-1}A^{\top}]+E[{w_{k-1}}^{\top}w_{k-1}]\\ &e_{k-1}= x_{k-1}-\hat{x}_{k-1} 真值减去后验与w_{k-1}相互独立\\ &=E[Ae_{k-1}{e_{k-1}}^{\top}A^{\top}]+E[w_{k-1}{w_{k-1}}^{\top}]\\ & 第二项为过程噪声的协方差Q\\ &=AP_{k-1}A^{\top}+Q \end{split}
Pk−=E[ek−ek−⊤]=E[(Aek−1+wk−1)(Aek−1+wk−1)⊤]=E[Aek−1ek−1⊤A⊤+Aek−1wk−1⊤+wk−1ek−1⊤A⊤+wk−1⊤wk−1]=E[Aek−1ek−1⊤A⊤]+E[Aek−1wk−1⊤]+E[wk−1ek−1⊤A⊤]+E[wk−1⊤wk−1]ek−1=xk−1−x^k−1真值减去后验与wk−1相互独立=E[Aek−1ek−1⊤A⊤]+E[wk−1wk−1⊤]第二项为过程噪声的协方差Q=APk−1A⊤+Q
总结: KF 五条公式
预测
- 先验 x ^ k − = A x ^ k − 1 + B u k − 1 \hat{x}^{-}_k=A\hat{x}_{k-1}+Bu_{k-1} x^k−=Ax^k−1+Buk−1
- 先验协方差: P k − = A P k − 1 A ⊤ + Q P^{-}_k=AP_{k-1}A^{\top}+Q Pk−=APk−1A⊤+Q
校正
- 卡尔曼增益:
K = P k − H ⊤ H P k − H ⊤ + R K = \frac{P^-_kH^{\top}}{HP^{-}_kH^{\top}+R} K=HPk−H⊤+RPk−H⊤ - 后验估计
x ^ k = x ^ k − + K ( z k − H x ^ k − ) \hat{x}_k=\hat{x}^-_k+K(z_k-H\hat{x}^{-}_k) x^k=x^k−+K(zk−Hx^k−) - 更新误差协方差:
P k = P k − − K H P k − − P k − H ⊤ K ⊤ + K H P k − H ⊤ K ⊤ + K R K ⊤ = P k − − K H P k − − P k − H ⊤ K ⊤ + K ( H P k − H ⊤ + R ) K ⊤ 带入 K = P k − − K H P k − \begin{split} P_k &= P^-_k-KHP^-_k-P^-_kH^{\top}K^{\top}+KHP^{-}_kH^{\top}K^{\top}+KRK^{\top}\\ &= P^-_k-KHP^-_k-P^-_kH^{\top}K^{\top}+K(HP^{-}_kH^{\top}+R)K^{\top}\\ & 带入K\\ &= P^{-}_k-KHP^{-}_k \end{split} Pk=Pk−−KHPk−−Pk−H⊤K⊤+KHPk−H⊤K⊤+KRK⊤=Pk−−KHPk−−Pk−H⊤K⊤+K(HPk−H⊤+R)K⊤带入K=Pk−−KHPk−
(如果侵权,请联系, 会立刻删除)
关于矩阵迹的思考:
上面的推导中出现了对协方差矩阵的迹求导并令其为0的转折点, 那么矩阵迹究竟是什么含义呢? 令其为0有意味着什么?
链接
知乎解释
迹为体积变化(行列式)的速度, 令其为0, 意味协方差矩阵在递归的过程中已经接近不变了, 对于一个不再变化的误差,类似稳态误差,直接给予补偿即可把误差消去.
扫一扫
563
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
