卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。本文将简要介绍卡尔曼滤波的原理及推导。
什么是卡尔曼滤波
首先定义问题：对于某一系统，知道当前状态 Xt <script type="math/tex" id="MathJax-Element-440">X_t</script>，存在以下两个问题：
- 经过时间 △t <script type="math/tex" id="MathJax-Element-441">\triangle t</script>后，下个状态 Xt+1 <script type="math/tex" id="MathJax-Element-442">X_{t+1}</script>如何求出？
- 假定已求出 Xt+1 <script type="math/tex" id="MathJax-Element-443">X_{t+1}</script>，在 t+1 <script type="math/tex" id="MathJax-Element-444">t+1</script>时刻收到传感器的非直接信息 Zt+1 <script type="math/tex" id="MathJax-Element-445">Z_{t+1}</script>，如何对状态 Xt+1 <script type="math/tex" id="MathJax-Element-446">X_{t+1}</script>进行更正？
这两个问题正是卡尔曼滤波要解决的问题，形式化两个问题如下：
- 预测未来
- 修正当下
下面，将以机器人导航为例，从预测未来和修正当下两个角度介绍卡尔曼滤波器。
卡尔曼滤波的原理
问题场景如下：一个机器人，我们想知道它实时的状态 x⃗ <script type="math/tex" id="MathJax-Element-509">\vec{x}</script>，同时也想做到预测未来和修正当下这两件事。
其状态 x <script type="math/tex" id="MathJax-Element-510">x</script>表示为一维大小为2的向量，元素分别表示位置信息与速度信息：
x⃗ =[pv]
<script type="math/tex; mode=display" id="MathJax-Element-535">\vec{x} = \begin{bmatrix} p\\ v \end{bmatrix}</script>
可是状态 x <script type="math/tex" id="MathJax-Element-536">x</script>不一定是精准的，其不确定性用协方差表示：
Pk=[ΣppΣvpΣpvΣvv]
<script type="math/tex; mode=display" id="MathJax-Element-543"> P_k = \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix}</script>
预测未来
只考虑自身状态
只考虑自身状态的情况下，根据物理公式，可得：
pkvk=pk−1+Δt=vk−1vk−1
<script type="math/tex; mode=display" id="MathJax-Element-591">\begin{split} \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + \Delta t &\color{royalblue}{v_{k-1}} \\ \color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} \end{split}</script>
用矩阵表示如下：
x̂ k=[10Δt1]x̂ k−1=Fkx̂ k−1
<script type="math/tex; mode=display" id="MathJax-Element-629">\begin{align} \color{deeppink}{\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \end{align}</script>
在状态变化的过程中引入了新的不确定性，根据协方差的乘积公式可得：
Cov(x)Cov(Ax)=Σ=AΣAT
<script type="math/tex; mode=display" id="MathJax-Element-631">\begin{equation} \begin{split} Cov(x) &= \Sigma\\ Cov(\color{firebrick}{\mathbf{A}}x) &= \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T \end{split} \label{covident} \end{equation}</script>
x̂ kPk=Fkx̂ k−1=FkPk−1FTk
<script type="math/tex; mode=display" id="MathJax-Element-587">\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T \end{split} \end{equation}</script>
考虑外部状态
外部状态，这里以加速度为例，引入变量 a <script type="math/tex" id="MathJax-Element-646">\color{darkorange}{a}</script> ( uk→ <script type="math/tex" id="MathJax-Element-647">\color{darkorange}{\vec{\mathbf{u}_k}}</script>)。
pkvk=pk−1+Δt=vk−1+vk−1+12aΔt2aΔt
<script type="math/tex; mode=display" id="MathJax-Element-654">\begin{split} \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + {\Delta t} &\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} {\Delta t}^2 \\ \color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} + & \color{darkorange}{a} {\Delta t} \end{split}</script>
x̂ k=Fkx̂ k−1+⎡⎣⎢⎢Δt22Δt⎤⎦⎥⎥a=Fkx̂ k−1+Bkuk→
<script type="math/tex; mode=display" id="MathJax-Element-689">\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \color{darkorange}{a} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \end{split} \end{equation}</script>
同时，环境仍然存在我们无法刻画的误差，以 Qk <script type="math/tex" id="MathJax-Element-690">{\color{mediumaquamarine}{\mathbf{Q}_k}}</script>表示，最终的预测公式如下：
x̂ kPk=Fkx̂ k−1+Bkuk→=FkPk−1FTk+Qk
<script type="math/tex; mode=display" id="MathJax-Element-990">\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k} \end{split} \end{equation}</script>
从上述式子可见：
- 新的最优估计 <script type="math/tex" id="MathJax-Element-991">\color{deeppink}{\textbf{新的最优估计}}</script>是 之前最优估计 <script type="math/tex" id="MathJax-Element-992">\color{royalblue} {\textbf{之前最优估计}}</script>的预测加上 已知的外界影响 <script type="math/tex" id="MathJax-Element-993">\color{darkorange}{\textbf{已知的外界影响}}</script>的修正。
- 新的不确定度 <script type="math/tex" id="MathJax-Element-994">\color{deeppink}{\textbf{新的不确定度}}</script>是 预测的不确定度 <script type="math/tex" id="MathJax-Element-995">\color{royalblue} {\textbf{预测的不确定度}}</script>加上 环境的不确定度 <script type="math/tex" id="MathJax-Element-996">\color{mediumaquamarine}{\textbf{环境的不确定度}}</script>。
修正当下
我们已得到 x̂ k,Pk <script type="math/tex" id="MathJax-Element-1427">\color{deeppink}{\mathbf{\hat{x}}_k, P_k}</script>，下面要通过观测到的测量值 zk→ <script type="math/tex" id="MathJax-Element-1428">\color{yellowgreen}{\vec{\mathbf{z}_k}}</script>对 x̂ k,Pk <script type="math/tex" id="MathJax-Element-1429">{\mathbf{\hat{x}}_k, P_k}</script>进行更新。
因为 x̂ k,Pk <script type="math/tex" id="MathJax-Element-1430">\color{deeppink}{\mathbf{\hat{x}}_k, P_k}</script>和 zk→ <script type="math/tex" id="MathJax-Element-1431">\color{yellowgreen}{\vec{\mathbf{z}_k}}</script>的数据尺度不一定相同，例如 x̂ k,Pk <script type="math/tex" id="MathJax-Element-1432">\color{deeppink}{\mathbf{\hat{x}}_k, P_k}</script>包含了 笛卡尔 <script type="math/tex" id="MathJax-Element-1433">\color{deeppink}{\textbf{笛卡尔}}</script>的坐标信息，使用radar得到的 zk→ <script type="math/tex" id="MathJax-Element-1434">\color{yellowgreen}{\vec{\mathbf{z}_k}}</script>则包含 极坐标 <script type="math/tex" id="MathJax-Element-1435">\color{yellowgreen}{\textbf{极坐标}}</script>信息。所以首先应该把两者放在相同的尺度下去比较，尺度转换使用 Hk <script type="math/tex" id="MathJax-Element-1436">\mathbf{H}_k</script>将预测信息转化为测量信息的尺度。
μ⃗ expectedΣexpected=Hkx̂ k=HkPkHTk
<script type="math/tex; mode=display" id="MathJax-Element-1501">\begin{equation} \begin{aligned} \vec{\mu}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{P}_k} \mathbf{H}_k^T \end{aligned} \end{equation}</script>
这样一来，便得到测量尺度上的两个分布：
- 测量值的分布 (x,μ1,σ1) <script type="math/tex" id="MathJax-Element-1502">\mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1})</script>
- 预测值变换后的分布 (x,μ0,σ0) <script type="math/tex" id="MathJax-Element-1503">\mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) </script>
下面一个问题就是如何用这个两个分布组成新的分布。
(x,μ0,σ0)⋅(x,μ1,σ1)=?(x,μ′,σ′)
<script type="math/tex; mode=display" id="MathJax-Element-1512">\begin{equation} \mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) \cdot \mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, \color{royalblue}{\mu’}, \color{mediumblue}{\sigma’}) \end{equation}</script>
简单的一维情况如下：
k=σ20σ20+σ21
<script type="math/tex; mode=display" id="MathJax-Element-1549">\begin{equation} \color{purple}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} \end{equation}</script>
高维情况下，针对测量值分布 (μ1,Σ1)=(zk→,Rk) <script type="math/tex" id="MathJax-Element-1550">(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})</script>与预测值的变化分布 (μ0,Σ0)=(Hkx̂ k,HkPkHTk) <script type="math/tex" id="MathJax-Element-1551">(\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})</script>组合的高斯分布如下：
x̂ ′kP′k=x̂ k=Pk+–K′(zk→–Hkx̂ k)K′HkPk
<script type="math/tex; mode=display" id="MathJax-Element-1556">\begin{equation} \begin{split} \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \color{royalblue}{\mathbf{P}_k’} &= \color{deeppink}{\mathbf{P}_k} & – & \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k} \end{split} \end{equation}</script>
K′=PkHTk(HkPkHTk+Rk)−1
<script type="math/tex; mode=display" id="MathJax-Element-1558">\begin{equation} \color{purple}{\mathbf{K}’} = \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \end{equation}</script>
总结
预测未来
- 输入：过去的最优状态（ x̂ k−1 <script type="math/tex" id="MathJax-Element-1858">\color{royalblue}{\mathbf{\hat{x}}_{k-1}}</script>， Pk−1 <script type="math/tex" id="MathJax-Element-1859">\color{royalblue}{\mathbf{P}_{k-1}}</script>）、外界对过程的影响 uk→ <script type="math/tex" id="MathJax-Element-1860">\color{darkorange}{\vec{\mathbf{u}_k}}</script>，环境的不确定度 Qk <script type="math/tex" id="MathJax-Element-1861">\color{mediumaquamarine}{\mathbf{Q}_k} </script>。
- 输出：预测的最优状态（ x̂ k <script type="math/tex" id="MathJax-Element-1862">\color{deeppink}{\mathbf{\hat{x}}_{k}}</script>， Pk <script type="math/tex" id="MathJax-Element-1863">\color{deeppink}{\mathbf{P}_{k}}</script>）。
- 其他：对过程的描述（ Fk <script type="math/tex" id="MathJax-Element-1864"> \mathbf{F_k} </script>， Bk <script type="math/tex" id="MathJax-Element-1865">\mathbf{B_k} </script>）跟时间有关。
x̂ kPk=Fkx̂ k−1+Bkuk→=FkPk−1FTk+Qk
<script type="math/tex; mode=display" id="MathJax-Element-1623">\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k} \end{split} \end{equation}</script>
修正当下
- 输入：预测的最优状态（ x̂ k <script type="math/tex" id="MathJax-Element-1993">\color{deeppink}{\mathbf{\hat{x}}_{k}}</script>， Pk <script type="math/tex" id="MathJax-Element-1994">\color{deeppink}{\mathbf{P}_{k}}</script>），测量的状态分布 (zk→,Rk) <script type="math/tex" id="MathJax-Element-1995"> (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})</script>，预测到测量的变换矩阵 Hk <script type="math/tex" id="MathJax-Element-1996">\color{deeppink}{\mathbf{H}_k} </script>。
- 输出：经过测量修正的最优状态（ x̂ ′k <script type="math/tex" id="MathJax-Element-1997">\color{royalblue}{\mathbf{\hat{x}}_{k}'}</script>， P′k <script type="math/tex" id="MathJax-Element-1998">\color{royalblue}{\mathbf{P}_{k}'}</script>）。
x̂ ′kP′k=x̂ k=Pk+–K′(zk→–Hkx̂ k)K′HkPk
<script type="math/tex; mode=display" id="MathJax-Element-1565">\begin{equation} \begin{split} \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \color{royalblue}{\mathbf{P}_k’} &= \color{deeppink}{\mathbf{P}_k} & – & \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k} \end{split} \end{equation}</script>
K′=PkHTk(HkPkHTk+Rk)−1
<script type="math/tex; mode=display" id="MathJax-Element-2011">\begin{equation} \color{purple}{\mathbf{K}’} = \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \end{equation}</script>
卡尔曼滤波需要内存少，计算速度快，适合实时性情况与嵌入式设备的需要。
参考
- How a Kalman filter works, in pictures
- Kalman Filter For Dummies
58
0
已为社区贡献7条内容
所有评论(0)