卡尔曼滤波公式推导
1.运动方程及观测方程
已知时刻
k
−
1
k-1
k−1位置状态向量为
X
k
−
1
X_{k-1}
Xk−1，下一时刻
k
k
k位置状态向量为
X
k
X_k
Xk，两者之间的转换关系如下:
X
k
=
F
k
X
k
−
1
+
n
m
X_k = F_kX_{k-1} + n_m
Xk=FkXk−1+nm
其中，
F
k
F_k
Fk表示从
k
−
1
k-1
k−1时刻到
k
k
k时刻之间位置状态向量的转移关系，
n
m
n_m
nm表示运动模型噪声，假设服从于正态分布
N
(
0
,
Q
)
N(0, Q)
N(0,Q)
此外，在不同的位置观测到不同的路标点
Z
k
Z_k
Zk，因此观测方程关系如下：
Z
k
=
H
k
X
k
+
n
w
Z_k = H_kX_k+n_w
Zk=HkXk+nw
其中，
H
x
H_x
Hx表示根据位置向量生成观测
Z
k
Z_k
Zk的观测矩阵，
n
w
n_w
nw表示观测噪声，假设服从于正态分布
N
(
0
,
R
)
N(0,R)
N(0,R)
联立运动方程和观测方程可得
X
k
=
F
k
X
k
−
1
+
n
m
Z
k
=
H
k
X
k
+
n
w
X_k = F_kX_{k-1} + n_m\\ Z_k = H_kX_k+n_w
Xk=FkXk−1+nmZk=HkXk+nw
需要注意的是，此处方程都是线性系统的表达形式，实际问题中， F k F_k Fk和 H k H_k Hk应当是关于 X X X的非线性函数，对于这种非线性系统，需要做线性化，即扩展卡尔曼滤波
2.目标函数构建
卡尔曼滤波实际上可以看做是优化问题的迭代求解，因此，以优化问题的角度看，应当先构建优化目标函数。
从结果来看，我们想要求解出满足运动方程和观测方程的最优位置状态向量
X
k
∗
X_k^*
Xk∗，因此，若假设
X
k
−
X_k^-
Xk−表示基于运动方程得到的预测位置，
X
k
+
X_k^+
Xk+表示基于观测方程得到的观测位置，那么，由于噪声不可测量，先忽略运动方程与观测方程之间的噪声，理想情况下目标函数应该是
L
(
X
k
)
=
(
X
k
−
(
X
k
−
+
X
k
+
)
/
2
)
T
(
X
k
−
(
X
k
−
+
X
k
+
)
/
2
)
L(X_k) = (X_k - (X_k^-+X_k^+)/2)^T(X_k - (X_k^-+X_k^+)/2)
L(Xk)=(Xk−(Xk−+Xk+)/2)T(Xk−(Xk−+Xk+)/2)
上式是个简单的最小二乘问题，可以求出解析解为
X
^
k
=
(
X
k
−
+
X
k
+
)
/
2
\hat{X}_k =(X_k^-+X_k^+)/2
X^k=(Xk−+Xk+)/2
其中，
X
k
−
=
F
k
X
k
−
1
X_k^-=F_kX_{k-1}
Xk−=FkXk−1，
X
k
+
=
H
k
−
1
Z
k
X_k^+=H_k^{-1}Z_k
Xk+=Hk−1Zk
显然，理想情况下最优结果是多个观测结果的均值，但在实际问题中，必然存在着噪声，各个观测之间的不确定性是不同的，因此优化估计得到的位置状态向量应该写成如下形式
X
^
k
=
X
k
−
+
G
k
(
X
k
+
−
X
k
−
)
\hat{X}_k = X_k^- + G_k(X_k^+-X_k^-)
X^k=Xk−+Gk(Xk+−Xk−)
其中，权重因子
G
k
G_k
Gk取值范围从0到1，趋于0时更相信运动方程预测的结果，趋于1时更相信观测方程的结果
进一步地，需要确定权重因子
G
k
G_k
Gk的值；显然，最优的权重因子应该使得估计位置状态向量
X
^
k
\hat{X}_k
X^k最接近真实位置状态向量
X
k
X_k
Xk，即构造目标函数如下：
L
(
G
k
)
=
E
(
(
X
k
−
X
^
k
)
T
(
X
k
−
X
^
k
)
)
=
E
(
e
k
T
e
k
)
L(G_k) = E((X_k-\hat{X}_k)^T(X_k-\hat{X}_k)) = E(e_k^Te_k)
L(Gk)=E((Xk−X^k)T(Xk−X^k))=E(ekTek)
其中，
e
k
=
X
k
−
X
^
k
=
X
k
−
X
k
−
−
G
k
(
X
k
+
−
X
k
−
)
=
X
k
−
（
I
−
G
k
）
X
k
−
1
−
G
k
H
k
−
1
Z
k
=
X
k
−
(
I
−
G
k
)
X
k
−
1
−
G
k
X
k
−
G
k
H
k
−
1
n
w
=
(
I
−
G
k
)
(
X
k
−
X
k
−
1
)
−
G
k
H
k
−
1
n
w
e_k = X_k-\hat{X}_k \\ = X_k - X_k^--G_k(X_k^+-X_k^-) \\ = X_k - （I-G_k）X_k^{-1} - G_kH_k^{-1}Z_k \\ = X_k - (I-G_k)X_k^{-1} - G_kX_k - G_kH_k^{-1}n_w \\ = (I-G_k)(X_k-X_k^{-1}) - G_kH_k^{-1}n_w
ek=Xk−X^k=Xk−Xk−−Gk(Xk+−Xk−)=Xk−（I−Gk）Xk−1−GkHk−1Zk=Xk−(I−Gk)Xk−1−GkXk−GkHk−1nw=(I−Gk)(Xk−Xk−1)−GkHk−1nw
即，将上式代入目标函数可写成
L
(
G
k
)
=
E
(
e
k
T
e
k
)
=
E
{
[
(
I
−
G
k
)
(
X
k
−
X
k
−
1
)
]
T
[
(
I
−
G
k
)
(
X
k
−
X
k
−
1
)
]
}
−
E
{
[
(
I
−
G
k
)
(
X
k
−
X
k
−
1
)
]
T
G
k
H
k
−
1
n
w
}
−
E
{
[
G
k
H
k
−
1
n
w
]
T
[
(
I
−
G
k
)
(
X
k
−
X
k
−
1
)
]
}
+
E
{
[
G
k
H
k
−
1
n
w
]
T
G
k
H
k
−
1
n
w
}
=
(
I
−
G
k
)
E
(
e
k
−
T
e
k
−
)
(
I
−
G
k
)
T
+
(
G
k
H
k
−
1
)
E
(
n
w
T
n
w
)
(
G
k
H
k
−
1
)
T
=
G
k
[
E
(
e
k
−
T
e
k
−
)
+
H
k
−
1
E
(
n
w
T
n
w
)
H
k
−
1
T
]
G
k
T
−
2
G
k
E
(
e
k
−
T
e
k
−
)
+
E
(
e
k
−
T
e
k
−
)
L(G_k) = E(e_k^Te_k) \\ = E\{[(I-G_k)(X_k-X_k^{-1})]^T[(I-G_k)(X_k-X_k^{-1})] \} - E\{[(I-G_k)(X_k-X_k^{-1})]^TG_kH_k^{-1}n_w\} \\- E\{[G_kH_k^{-1}n_w]^T[(I-G_k)(X_k-X_k^{-1})] \} + E\{[G_kH_k^{-1}n_w]^TG_kH_k^{-1}n_w\} \\ = (I-G_k)E({e_k^-}^Te_k^-)(I-G_k)^T + (G_kH_k^{-1})E(n_w^Tn_w)(G_kH_k^{-1})^T\\ = G_k[E({e_k^-}^Te_k^-) + H_k^{-1}E(n_w^Tn_w){H_k^{-1}}^T]G_k^T -2G_kE({e_k^-}^Te_k^-) + E({e_k^-}^Te_k^-)
L(Gk)=E(ekTek)=E{[(I−Gk)(Xk−Xk−1)]T[(I−Gk)(Xk−Xk−1)]}−E{[(I−Gk)(Xk−Xk−1)]TGkHk−1nw}−E{[GkHk−1nw]T[(I−Gk)(Xk−Xk−1)]}+E{[GkHk−1nw]TGkHk−1nw}=(I−Gk)E(ek−Tek−)(I−Gk)T+(GkHk−1)E(nwTnw)(GkHk−1)T=Gk[E(ek−Tek−)+Hk−1E(nwTnw)Hk−1T]GkT−2GkE(ek−Tek−)+E(ek−Tek−)
其中，
e
k
−
e_k^-
ek−实质上可以表示
X
k
−
X_k^-
Xk−协方差；第三个等号处，中间两项包含噪声的期望都为0，可以消掉
3.目标函数求解
对目标函数等式(9)求导可得
∂
L
∂
G
k
=
2
G
K
[
E
(
e
k
−
T
e
k
−
)
+
H
k
−
1
E
(
n
w
T
n
w
)
H
k
−
1
T
)
]
−
2
E
T
(
e
k
−
T
e
k
−
)
\frac{\partial L}{\partial G_k} = 2G_K[E({e_k^-}^Te_k^-) + H_k^{-1}E(n_w^Tn_w){H_k^{-1}}^T)]-2E^T({e_k^-}^Te_k^-)
∂Gk∂L=2GK[E(ek−Tek−)+Hk−1E(nwTnw)Hk−1T)]−2ET(ek−Tek−)
令导数为0可得，
G
K
=
[
E
(
e
k
−
T
e
k
−
)
+
H
k
−
1
E
(
n
w
T
n
w
)
H
k
−
1
T
)
]
−
1
E
T
(
e
k
−
T
e
k
−
)
=
P
k
−
P
k
−
+
H
k
−
1
R
H
k
−
1
T
G_K =[E({e_k^-}^Te_k^-) + H_k^{-1}E(n_w^Tn_w){H_k^{-1}}^T)]^{-1}E^T({e_k^-}^Te_k^-) \\ =\frac{P_k^-}{P_k^-+H_k^{-1}R{H_k^{-1}}^T}
GK=[E(ek−Tek−)+Hk−1E(nwTnw)Hk−1T)]−1ET(ek−Tek−)=Pk−+Hk−1RHk−1TPk−
其中，
P
k
P_k
Pk表示预测公式推导出协方差，
R
R
R表示噪声
n
w
n_w
nw的协方差，由此，求解出权重因子
G
k
G_k
Gk
需要注意的是， G k G_k Gk即常说的卡尔曼增益因子，这里形式与常规公式不一致，是因为等式(6)中将 H k H_k Hk放到括号里面，实质上是一致的
为了和一般公式一致，对等式(6)和等式(11)进行改写
X
^
k
=
X
k
−
+
G
k
(
Z
k
−
H
k
X
k
−
)
G
k
=
P
k
−
H
T
H
k
P
k
−
H
k
T
+
R
\hat{X}_k = X_k^-+G_k(Z_k-H_kX_k^-)\\ G_k = \frac{P_k^-H^T}{H_kP_k^-H_k^T+R}
X^k=Xk−+Gk(Zk−HkXk−)Gk=HkPk−HkT+RPk−HT
整理可得，
-
预测方程:
X k − = F k X ^ k − 1 P k − = F k P ^ k − 1 F k T + Q X_k^- = F_k\hat{X}_{k-1} \\ P_k^- = F_k\hat{P}_{k-1}F_k^T+Q\ Xk−=FkX^k−1Pk−=FkP^k−1FkT+Q -
卡尔曼增益:
G k = P k − H k T H P k − H T + R G_k = \frac{P_k^-H_k^T}{HP_k^-H^T+R} Gk=HPk−HT+RPk−HkT -
更新方程:
X ^ k = X k − + G k ( Z k − H k X k − ) P ^ k = ( I − G k H k ) P k − \hat{X}_k = X_k^- + G_k(Z_k-H_kX_k^-)\\ \hat{P}_k = (I-G_kH_k)P_k^- X^k=Xk−+Gk(Zk−HkXk−)P^k=(I−GkHk)Pk−
4.实际使用经验
- 初值怎么给
- 噪声方差如何确定
341
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
