0. 专栏导读
惯性导航在SLAM技术的应用中有着举足轻重的作用,从刚体的姿态解算,到今年来常用的多传感器融合都离不开惯性导航,而惯导又涉及很多的理论知识,为此开设本专栏的旨在汇总惯性导航的知识点.
专栏涵盖以下几个方面:
- 从KF到EKF(本文);
- 姿态解算;
- 基于流型(李代数)的ESKF及代码实现;
- 预积分(待发布).
1. 简介
卡尔曼滤波(Kalman Filter, KF)的思想十分新颖,引入了贝叶斯的思想解决信号的滤波问题,NASA也是采用了卡尔曼滤波预测飞船的轨迹.也正因为KF极先进的思想,KF适用于广泛的领域.如今很多工程实现中都有所应用.
本文将从KF从发,介绍其思想及方法,进一步推广至适用于非线性的扩展卡尔曼滤波(EKF),最后再到误差卡尔曼滤波(ESKF).
2. 卡尔曼滤波
上面提到卡尔曼滤波的核心思想是基于贝叶斯的思想,贝叶斯公式描述了一个我们日常碰到的问题,即在已知其它条件,如何得出更准确的估计?
贝叶斯公式
P
(
X
∣
Z
)
=
P
(
X
)
P
(
Z
∣
X
)
P
(
Z
)
(2.1)
P(X|Z) = \frac{P(X)P(Z|X)}{P(Z)} \tag{2.1}
P(X∣Z)=P(Z)P(X)P(Z∣X)(2.1)
上面的 X X X 就是人们经常想知道或者预测的一个状态量,好比天气预报,我们永远无法得知未来实际的天气情况,但我们基于当前的温湿度/地势环境等已知条件对明天的天气进行估计,这些可被观测的量即 Z Z Z,贝叶斯反映的就是这么一个过程.
2.1 数学模型
卡尔曼滤波的数学模型建立在马尔科夫的假设,即当前时刻的状态只与上一时刻有关,因此第k时刻的状态 x x x 可由下面运动方程(状态方程)预测:
x k = A x k − 1 + u k + w k (2.2) \boldsymbol{x}_k = \boldsymbol{A} \boldsymbol{x}_{k-1} + \boldsymbol{u}_k + \boldsymbol{w}_k \tag{2.2} xk=Axk−1+uk+wk(2.2)
上式中
- A \boldsymbol{A} A 是状态转移转移矩阵,反映上一时刻与当前时刻的(线性)变换关系;
- u k \boldsymbol{u}_k uk 是输入,已知量且可被认为是无噪声的;
- w k w_k wk为白噪声;
假如我们有其它手段得到一个观测量 z k z_k zk (例如我们有一些传感器输出的数据),而该数据与预测的状态存在下面的关系,称为观测方程:
z k = H x k + v k (2.3) \boldsymbol{z}_k = \boldsymbol{H} \boldsymbol{x}_{k} + \boldsymbol{v}_k \tag{2.3} zk=Hxk+vk(2.3)
上式中
- H \boldsymbol{H} H 是观测矩阵,反映状态与观测量的(线性)变化关系;
-
w
k
w_k
wk 和
v
k
v_k
vk 均为白噪声, 卡尔曼滤波均假设其服从 零均值高斯分布:
w k = N ( 0 , R ) , v k = N ( 0 , Q ) , w_k = N(0, \boldsymbol{R}), v_k = N(0, \boldsymbol{Q}), wk=N(0,R),vk=N(0,Q),
另外也认为带估计的状态量服从高斯分布.根据贝叶斯公式以及 高斯分布的性质(两个高斯分布相乘仍为高斯分布) 即可推导得出后验概率 P ( x ^ k ∣ z 1 : k ) P(\hat{\boldsymbol{x}}_k | \boldsymbol{z}_{1:k}) P(x^k∣z1:k) 分布.
上面是一张揭示卡尔曼滤波本质的示意图,用一种可能不太严谨的说法说,卡尔曼滤波就是为状态的预测和观测分别计算权重,然后最后得到的最优估计值就是"两者"的加权和.具体的推导过程不再详述,推荐看大佬们的文献和视频:
- 徐亦达机器学习：Kalman Filter 卡尔曼滤波 : 推导极其详细
- 视觉SLAM14讲:高博的推导方法更容易理解,更快捷.
2.2 算法流程
卡尔曼滤波的实现分为两步:
-
预测
x ˉ k = A x ^ k − 1 + u k P ˉ k = A P ^ k − 1 A T + R (2.4) \begin{aligned} \bar{x}_k &= \boldsymbol{A} \hat{\boldsymbol{x}}_{k-1} + \boldsymbol{u}_k \\ \bar{\boldsymbol{P}}_k &= \boldsymbol{A} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}^T + \boldsymbol{R} \tag{2.4} \end{aligned} xˉkPˉk=Ax^k−1+uk=AP^k−1AT+R(2.4)
从上面的公式可以发现所谓的"预测"所做的不过是按照运动方程更新了状态量的均值与方差.这里说句题外话,最初笔者接触卡尔曼滤波是在时间序列的预测(拟合),当时在搜索时发现这个算法具有预测能力,实则它的预测是基于已有运动方程的前提下,不然卡尔曼滤波不具备任何预测或拟合的能力… -
更新
K = P ˉ k H k T ( H k P ˉ k H k T + Q ) − 1 x ^ k = x ˉ k + K ( z k − H x ˉ k ) P ^ k = ( I − K H ) P ˉ k (2.5) \begin{aligned} \boldsymbol{K} &= \bar{\boldsymbol{P}}_k \boldsymbol{H}_k^T ( \boldsymbol{H}_k \bar{\boldsymbol{P}}_k \boldsymbol{H}_k^T+\boldsymbol{Q})^{-1} \\ \hat{\boldsymbol{x}}_k &= \bar{\boldsymbol{x}}_k + \boldsymbol{K}\left(\boldsymbol{z}_k-\boldsymbol{H} \bar{\boldsymbol{x}}_k\right) \\ \hat{\boldsymbol{P}}_k &= \left(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H} \right)\bar{\boldsymbol{P}}_k \tag{2.5} \end{aligned} Kx^kP^k=PˉkHkT(HkPˉkHkT+Q)−1=xˉk+K(zk−Hxˉk)=(I−KH)Pˉk(2.5)
更新的过程就是根据观测方程与预测得到的状态估计值计算一个增益 K \boldsymbol{K} K (也可认为是一个权重),然后基于这个增益同时修正更新得到的状态均值和方差.
3. 扩展卡尔曼滤波
上面介绍卡尔曼滤波的理念十分好,既有我们对系统建模而得到的状态估计值(运动方程),又结合了观测数据对其进行修正,然而最大的遗憾是其假设系统是线性的,我们所处的世界却是充满了非线性,那么我们是否有办法可以将卡尔曼滤波的思想应用到非线性问题中?
3.1 数学模型
相信很多接触过非线性问题线性化处理的同学们很自然想到的是泰勒展开,扩展卡尔曼滤波(Extended Kalman Filter)正是使用了泰勒展开对非线性问题进行线性化操作.
具体方法如下,运动方程和观测方程不再是线性关系:
x
k
=
f
(
x
k
−
1
,
u
k
)
z
k
=
h
(
x
k
)
(3.1)
\begin{align} \boldsymbol{x}_k &= f\left(\boldsymbol{x}_{k-1},\boldsymbol{u}_k\right) \\ \boldsymbol{z}_k &= h(\boldsymbol{x}_k) \end{align} \tag{3.1}
xkzk=f(xk−1,uk)=h(xk)(3.1)
遗憾的是,我们无法知晓真实的
x
k
−
1
\boldsymbol{x}_{k-1}
xk−1 ,只有关于其的一个分布情况,自然
x
k
\boldsymbol{x}_k
xk 和
z
k
\boldsymbol{z}_k
zk 也同样是一个分布,那现在的问题是如何通过一个已知的分布以及非线性的关系得知另一个的分布?
x
k
−
1
\boldsymbol{x}_{k-1}
xk−1 的均值
x
^
k
−
1
\hat{\boldsymbol{x}}_{k-1}
x^k−1(即上一时刻的最优估计),将式3.1的运动方程在
x
^
k
−
1
\hat{\boldsymbol{x}}_{k-1}
x^k−1 处展开,并只保留一阶项有:
x k ≈ f ( x ^ k − 1 , u k ) + ∂ f ∂ x k − 1 ∣ x ^ k − 1 ( x k − 1 − x ^ k − 1 ) + w k (3.2) \boldsymbol{x}_k\approx f\left(\hat{\boldsymbol{x}}_{k-1},\boldsymbol{u}_k\right)+\left.\frac{\partial f}{\partial\boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}}(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1})+\boldsymbol{w}_k \tag{3.2} xk≈f(x^k−1,uk)+∂xk−1∂f x^k−1(xk−1−x^k−1)+wk(3.2)
记上式的偏导符号为:
F
=
∂
f
∂
x
k
−
1
∣
x
^
k
−
1
(3.3)
\boldsymbol{F} = \left.\frac{\partial f}{\partial\boldsymbol{x}_{k-1}} \right|_{\hat{\boldsymbol{x}}_{k-1}} \tag{3.3}
F=∂xk−1∂f
x^k−1(3.3)
从式3.2不难发现:
- 第一项其实反映 x k \boldsymbol{x}_k xk 的均值,即k时刻的状态预测值 x ˉ k \bar{\boldsymbol{x}}_k xˉk;
- 第二项和第三项是运动方程将 x k − 1 \boldsymbol{x}_{k-1} xk−1 的不确定性"传递"到 x k \boldsymbol{x}_{k} xk 上,即对应协方差.
所以,通过预测方程我们可以得到:
P
(
x
k
∣
x
0
,
u
1
:
k
,
z
0
:
k
-
1
)
=
N
(
f
(
x
^
k
−
1
,
u
k
)
,
F
P
^
k
−
1
F
T
+
R
k
)
(3.4)
P\left(\boldsymbol{x}_k|\boldsymbol{x}_0,\boldsymbol{u}_{1:k},\boldsymbol{z}_{0:k\text{-}1}\right)=N(f\left(\hat{\boldsymbol{x}}_{k-1},\boldsymbol{u}_k\right),\boldsymbol{F}\hat{\boldsymbol{P}}_{k-1}\boldsymbol{F}^\mathrm{T}+\boldsymbol{R}_k) \tag{3.4}
P(xk∣x0,u1:k,z0:k-1)=N(f(x^k−1,uk),FP^k−1FT+Rk)(3.4)
同样的,观测方程也需要线性化,将式3.1的观测方程在预测得到均值处 ( x ˉ k \bar{\boldsymbol{x}}_k xˉk) 泰勒展开,并只保留一阶项有:
z k ≈ h ( x ˉ k ) + ∂ h ∂ x k ∣ x ˉ k ( x k − x ˉ k ) + n k (3.5) \boldsymbol{z}_k \approx h\left.(\bar{\boldsymbol{x}}_k)+\frac{\partial h}{\partial\boldsymbol{x}_k}\right|_{\boldsymbol{\bar{\boldsymbol{x}}}_k}(\boldsymbol{x}_k-\bar{\boldsymbol{x}}_k)+\boldsymbol{n}_k \tag{3.5} zk≈h(xˉk)+∂xk∂h xˉk(xk−xˉk)+nk(3.5)
记上式的偏导符号为:
H = ∂ h ∂ x k ∣ x ˉ k (3.6) \boldsymbol{H} = \left.\frac{\partial h}{\partial\boldsymbol{x}_k}\right|_{\boldsymbol{\bar{\boldsymbol{x}}}_k} \tag{3.6} H=∂xk∂h xˉk(3.6)
根据式3.5的关系,假如已知
x
k
\boldsymbol{x}_k
xk, 不能得出
z
k
\boldsymbol{z}_k
zk 的分布:
P
(
z
k
∣
x
k
)
=
N
(
h
(
x
ˉ
k
)
+
H
(
x
k
−
x
ˉ
k
)
,
Q
k
)
(3.7)
P\left(\boldsymbol{z}_k|\boldsymbol{x}_k\right)=N(h\left(\bar{\boldsymbol{x}}_k\right)+\boldsymbol{H}\left(\boldsymbol{x}_k-\bar{\boldsymbol{x}}_k\right),\boldsymbol{Q}_k) \tag{3.7}
P(zk∣xk)=N(h(xˉk)+H(xk−xˉk),Qk)(3.7)
在这里补充一句,式3.5的第二项之所以不像式3.2中的第二项那样作为不确定性项,是因为我们关心的是
P
(
z
k
∣
x
k
)
P\left(\boldsymbol{z}_k|\boldsymbol{x}_k\right)
P(zk∣xk),这时候
x
k
\boldsymbol{x}_k
xk 是已知的,因此仅会影响均值,而不会影响协方差.
3.2 算法流程
有了式3.4和3.7的概率分布,我们就可以运用贝叶斯公式2.1计算k时刻下的后验分布了,这个过程不再叙述,直接给出结论
- 预测
x ˉ k = F x ^ k − 1 + u k P ˉ k = F P ^ k − 1 F T + R (3.8) \begin{aligned} \bar{x}_k &= \boldsymbol{F} \hat{\boldsymbol{x}}_{k-1} + \boldsymbol{u}_k \\ \bar{\boldsymbol{P}}_k &= \boldsymbol{F} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}^T + \boldsymbol{R} \tag{3.8} \end{aligned} xˉkPˉk=Fx^k−1+uk=FP^k−1FT+R(3.8) - 更新
K = P ˉ k H k T ( H k P ˉ k H k T + Q ) − 1 x ^ k = x ˉ k + K ( z k − H x ˉ k ) P ^ k = ( I − K H ) P ˉ k (3.9) \begin{aligned} \boldsymbol{K} &= \bar{\boldsymbol{P}}_k \boldsymbol{H}_k^T ( \boldsymbol{H}_k \bar{\boldsymbol{P}}_k \boldsymbol{H}_k^T+\boldsymbol{Q})^{-1} \\ \hat{\boldsymbol{x}}_k &= \bar{\boldsymbol{x}}_k + \boldsymbol{K}\left(\boldsymbol{z}_k-\boldsymbol{H} \bar{\boldsymbol{x}}_k\right) \\ \hat{\boldsymbol{P}}_k &= \left(\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H} \right)\bar{\boldsymbol{P}}_k \tag{3.9} \end{aligned} Kx^kP^k=PˉkHkT(HkPˉkHkT+Q)−1=xˉk+K(zk−Hxˉk)=(I−KH)Pˉk(3.9)
不难发现,其实EKF本质上和KF一样,唯一区别是用雅可比矩阵替代原来线性变换关系,并用线性关系刻画不确定性的传递关系.
4313
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
