卡尔曼滤波(KF),扩展卡尔曼滤波(EKF)
声明:本文仅作为个人复习与分享,里面很多内容是对其他博文的整理,在文章中均有注明。本人小白一枚,文章中如有错误,希望各位提醒!
1.卡尔曼滤波(KF)
1.1 卡尔曼滤波的模型假设
Kalman滤波所解决的问题,是对一个动态变化的系统的状态跟踪的问题,基本的模型假设包括:
1.系统的状态方程是线性的
2.观测方程是线性的
3.过程噪声符合零均值高斯分布
4.观测噪声符合零均值高斯分布
状态方程
x
t
=
A
t
x
t
−
1
+
B
t
u
t
+
W
t
x_{t}=A_{t}x_{t-1}+B_{t}u_{t}+W _{t}
xt=Atxt−1+Btut+Wt
W
t
∼
N
(
0
,
Q
t
)
W_{t} \sim N(0,Q_{t})
Wt∼N(0,Qt)
观测方程
z
t
=
H
t
x
t
+
V
t
z_{t}=H_{t}x_{t}+V{t}
zt=Htxt+Vt
V
t
∼
N
(
0
,
R
t
)
V_{t} \sim N(0,R_{t})
Vt∼N(0,Rt)
1.2 卡尔曼滤波算法
预测过程
x
^
t
′
=
A
t
x
^
t
−
1
+
B
t
u
t
\hat x_{t}^{'}=A_{t}\hat x_{t-1}+B_{t}u_{t}
x^t′=Atx^t−1+Btut 预测t时刻的状态
P
t
′
=
A
t
P
t
−
1
A
t
T
+
Q
t
P_{t}^{'}=A_{t}P_{t-1}A_{t}^{T}+Q_{t}
Pt′=AtPt−1AtT+Qt 预测方差矩阵
更新过程
K
t
=
P
t
′
H
t
T
(
H
t
P
t
′
H
t
T
+
R
t
)
−
1
K_{t}=P_{t}^{'}H_{t}^{T}(H_{t}P_{t}^{'}H_{t}^{T}+R_{t})^{-1}
Kt=Pt′HtT(HtPt′HtT+Rt)−1 计算卡尔曼增益
x
^
t
=
x
^
t
′
+
K
t
(
z
t
−
H
t
x
^
t
′
)
\hat x_{t}=\hat x_{t}^{'}+K_{t}(z_{t}-H_{t}\hat x_{t}^{'})
x^t=x^t′+Kt(zt−Htx^t′) 基于观测进行状态更新
P
t
=
(
I
−
K
t
H
t
)
P
t
′
P_{t}=(I-K_{t}H_{t})P_{t}^{'}
Pt=(I−KtHt)Pt′ 计算更新状态的方差矩阵
其中,
x
^
t
\hat x_{t}
x^t, n维向量,表示t时刻卡尔曼估计值
x
^
t
′
\hat x_{t}^{'}
x^t′, n维向量,表示t时刻预测值
P
t
P_{t}
Pt,
n
∗
n
n*n
n∗n方差矩阵,表示t时刻被观测的n个状态的方差
P
t
′
P_{t}^{'}
Pt′ ,
n
∗
n
n*n
n∗n方差矩阵,表示t时刻预测的n个状态的方差
u
t
u_{t}
ut, l 维向量,表示t时刻的输入
z
t
z_{t}
zt, m维向量,表示t时刻的观测
A
t
A_{t}
At,
m
∗
m
m*m
m∗m矩阵,表示状态转移矩阵
B
t
B_{t}
Bt,
m
∗
n
m*n
m∗n矩阵,表示输入增益矩阵
H
t
H_{t}
Ht,
m
∗
n
m*n
m∗n矩阵,表示观测矩阵
Q
t
Q_{t}
Qt,
n
∗
n
n*n
n∗n矩阵,表示过程噪音
W
t
W _{t}
Wt的方差矩阵
R
t
R_{t}
Rt,
m
∗
m
m*m
m∗m矩阵,表示观测噪音
V
t
V_{t}
Vt的方差矩阵
注:本节引用了 http://www.cnblogs.com/ycwang16/p/5999034.html
1.3 推导过程
真实值与预测值之间的误差为
e
t
′
=
x
t
−
x
^
t
′
e_{t}^{'}=x_{t}- \hat x_{t}^{'}
et′=xt−x^t′
预测误差协方差矩阵为
P
t
′
=
E
[
e
t
′
e
t
′
T
]
=
E
[
(
x
t
−
x
^
t
′
)
(
x
t
−
x
^
t
′
)
T
]
P_{t}^{'}=E[e_{t}^{'}e_{t}^{'T}]=E[(x_{t}- \hat x_{t}^{'})(x_{t}- \hat x_{t}^{'})^{T}]
Pt′=E[et′et′T]=E[(xt−x^t′)(xt−x^t′)T]
真实值与卡尔曼估计值之间的误差为:
e
t
=
x
t
−
x
^
t
=
x
t
−
(
x
^
t
′
+
K
t
(
H
x
t
+
V
t
−
H
x
^
t
′
)
)
=
(
I
−
K
t
H
)
(
x
t
−
x
^
t
′
)
−
K
t
V
t
e_{t}=x_{t} - \hat x_{t}=x_{t}-(\hat x_{t}^{'}+K_{t}(Hx_{t}+V_{t}-H\hat x_{t}^{'})) \\=(I-K_{t}H)(x_{t} - \hat x_{t}^{'})-K_{t}V_{t}
et=xt−x^t=xt−(x^t′+Kt(Hxt+Vt−Hx^t′))=(I−KtH)(xt−x^t′)−KtVt
将
e
t
e_{t}
et代入卡尔曼估计误差协方差矩阵
P
t
=
E
[
e
t
e
t
T
]
P_{t}=E[e_{t}e_{t}^{T}]
Pt=E[etetT]
P
t
=
E
[
[
(
I
−
K
t
H
)
(
x
t
−
x
^
t
′
)
−
K
t
V
t
]
[
(
I
−
K
t
H
)
(
x
t
−
x
^
t
′
)
−
K
t
V
t
]
T
]
=
(
I
−
K
t
H
)
E
[
(
x
t
−
x
^
t
′
)
(
x
t
−
x
^
t
′
)
T
]
(
I
−
K
t
H
)
T
+
K
t
E
[
V
t
V
t
T
]
K
t
T
P_{t}=E[[(I-K_{t}H)(x_{t} - \hat x_{t}^{'})-K_{t}V_{t}][(I-K_{t}H)(x_{t} - \hat x_{t}^{'})-K_{t}V_{t}]^{T}] \\ =(I-K_{t}H)E[(x_{t} - \hat x_{t}^{'})(x_{t} - \hat x_{t}^{'})^{T}](I-K_{t}H)^{T}+K_{t}E[V_{t}V_{t}^{T}]K_{t}^{T}
Pt=E[[(I−KtH)(xt−x^t′)−KtVt][(I−KtH)(xt−x^t′)−KtVt]T]=(I−KtH)E[(xt−x^t′)(xt−x^t′)T](I−KtH)T+KtE[VtVtT]KtT
其中
E
[
V
t
V
t
T
]
=
R
t
E[V_{t}V_{t}^{T}]=R_{t}
E[VtVtT]=Rt,将预测误差协方差矩阵代入,得到
P
t
=
(
I
−
K
t
H
)
P
t
′
(
I
−
K
t
H
)
T
+
K
t
R
t
K
t
T
P_{t}=(I-K_{t}H)P_{t}^{'}(I-K_{t}H)^{T}+K_{t}R_{t}K_{t}^{T}
Pt=(I−KtH)Pt′(I−KtH)T+KtRtKtT
卡尔曼滤波本质是最小均方差估计,而均方差是
P
t
P_{t}
Pt的迹,于是我们有
t
r
(
P
t
)
=
t
r
(
P
t
′
)
−
2
t
r
(
K
t
H
P
t
′
)
+
t
r
(
K
t
(
H
P
t
′
H
T
+
R
t
)
K
t
T
)
tr(P_{t})=tr(P_{t}^{'})-2tr(K_{t}HP_{t}^{'})+tr(K_{t}(HP_{t}^{'}H^{T}+R_{t})K_{t}^{T})
tr(Pt)=tr(Pt′)−2tr(KtHPt′)+tr(Kt(HPt′HT+Rt)KtT)
最优估计
K
t
K_{t}
Kt使
t
r
(
P
t
)
tr(P_{t})
tr(Pt)最小,所以上式两边对
K
t
K_{t}
Kt求导
t
r
(
P
t
)
∂
K
t
=
−
∂
2
t
r
(
K
t
H
P
t
′
)
∂
K
t
+
∂
t
r
(
K
t
(
H
P
t
′
H
T
+
R
t
)
K
t
T
)
∂
K
t
\frac{tr(P_{t})}{\partial K_{t}}=-\frac{\partial 2tr(K_{t}HP_{t}^{'})}{\partial K_{t}}+\frac{\partial tr(K_{t}(HP_{t}^{'}H^{T}+R_{t})K_{t}^{T})}{\partial K_{t}}
∂Kttr(Pt)=−∂Kt∂2tr(KtHPt′)+∂Kt∂tr(Kt(HPt′HT+Rt)KtT)
利用矩阵微分公式(1)(2)
∂
t
r
(
A
B
)
∂
A
=
B
T
(
1
)
\frac{\partial tr(AB)}{\partial A}=B^{T} (1)
∂A∂tr(AB)=BT(1)
∂
t
r
(
A
B
A
T
)
∂
A
=
2
A
B
(
2
)
\frac{\partial tr(ABA^{T})}{\partial A}=2AB (2)
∂A∂tr(ABAT)=2AB(2)
可以得到
$$
t
r
(
P
t
)
∂
K
t
=
−
2
(
H
P
t
′
)
T
+
2
K
t
(
H
P
t
′
H
T
+
R
t
)
\frac{tr(P_{t})}{\partial K_{t}}=-2(HP_{t}^{'})^{T}+2K_{t}(HP_{t}^{'}H^{T}+R_{t})
∂Kttr(Pt)=−2(HPt′)T+2Kt(HPt′HT+Rt)
另上式等于0,可以得到卡尔曼增益:
K
t
=
P
t
′
H
t
T
(
H
t
P
t
′
H
t
T
+
R
t
)
−
1
K_{t}=P_{t}^{'}H_{t}^{T}(H_{t}P_{t}^{'}H_{t}^{T}+R_{t})^{-1}
Kt=Pt′HtT(HtPt′HtT+Rt)−1
下面我们计算
P
t
′
P_{t}^{'}
Pt′
P
t
′
=
E
[
(
x
t
−
x
^
t
′
)
(
x
t
−
x
^
t
′
)
T
]
=
E
[
(
A
x
t
−
1
+
B
u
t
+
w
t
−
A
x
^
t
−
1
−
B
u
t
)
(
A
x
t
−
1
+
B
u
t
+
w
t
−
A
x
^
t
−
1
−
B
u
t
)
T
]
=
E
[
(
A
(
x
t
−
1
−
x
^
t
−
1
)
+
w
t
)
(
A
(
x
t
−
1
−
x
^
t
−
1
)
+
w
t
)
T
]
=
E
[
(
A
(
e
t
−
1
)
(
A
(
e
t
−
1
)
T
]
+
E
[
w
t
w
t
T
]
=
A
P
t
−
1
A
T
+
Q
P_{t}^{'}=E[(x_{t}- \hat x_{t}^{'})(x_{t}- \hat x_{t}^{'})^{T}] \\= E[(Ax_{t-1}+Bu_{t}+w_{t}-A\hat x_{t-1}-Bu_{t})(Ax_{t-1}+Bu_{t}+w_{t}-A\hat x_{t-1}-Bu_{t})^{T}] \\= E[(A(x_{t-1}-\hat x_{t-1})+w_{t})(A(x_{t-1}-\hat x_{t-1})+w_{t})^{T}] \\=E[(A(e_{t-1})(A(e_{t-1})^{T}]+E[w_{t}w_{t}^{T}] \\ =AP_{t-1}A^{T}+Q
Pt′=E[(xt−x^t′)(xt−x^t′)T]=E[(Axt−1+But+wt−Ax^t−1−But)(Axt−1+But+wt−Ax^t−1−But)T]=E[(A(xt−1−x^t−1)+wt)(A(xt−1−x^t−1)+wt)T]=E[(A(et−1)(A(et−1)T]+E[wtwtT]=APt−1AT+Q
以上推导参考文章:https://blog.csdn.net/victor_zy/article/details/82862904
2.扩展卡尔曼滤波(EKF)
围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上项,得到一个近似的线性化模型,然后应用卡尔曼滤波完成状态估计。
系统方程
x
t
=
f
(
x
t
−
1
)
+
W
t
x_{t}=f(x_{t-1})+W_{t}
xt=f(xt−1)+Wt
z
t
=
h
(
x
t
−
1
)
+
V
t
−
1
z_{t}=h(x_{t-1})+V_{t-1}
zt=h(xt−1)+Vt−1
在扩展卡尔曼滤波中,状态的预测以及观测值的预测由非线性函数计算得出,线性卡尔曼滤波中的状态转移矩阵A阵和观测矩阵H阵由f和h函数的雅克比矩阵代替,假设状态有n维,则求法如下:
EKF的局限性:
- 当强非线性时EKF违背局部线性假设,当Taylor展开式中被忽略的高阶项带来的误差较大时,EKF可能会使结果发散;
2.由于EKF在线性化处理时需要用Jacobian矩阵,计算过程繁琐。所以,在满足线性系统、高斯白噪声、所有随机变量服从高斯(Gaussian)分布这3个假设条件时,EKF是最小方差准则下的次优滤波器,其性能依赖于局部非线性度。
注:本节文章参考
作者:回忆不能已 https://blog.csdn.net/qq_18163961/article/details/52505591
作者: Rap_God https://blog.csdn.net/u012936940/article/details/77249245
参考
[1] http://www.cnblogs.com/ycwang16/p/5999034.html
[2] https://blog.csdn.net/victor_zy/article/details/82862904
[3] https://blog.csdn.net/u010720661/article/details/63253509