图2. 汽车运动状态变换示意图
2.1 理论预测
第一种
是通过理论估计来预测汽车在
\(t\)
时刻的状态，假如我们已经知道汽车在
\(t-1\)
时刻的位置和速度，以及匀加速运动的加速度
\(a\)
，那么，我们可以根据匀加速运动定律对汽车在
\(t\)
时刻的位置和速度进行一个理想（没有误差）的估计：
\[{d_{t|t - 1}} = {d_{t - 1|t - 1}} + {v_{t - 1|t - 1}}\Delta t + \frac{1}{2}a\Delta {t^2}\quad\quad(1)
\]
同样的，我们也可以对
\(t\)
时刻的汽车速度做一个理想的（没有误差）估计：
\[{v_{t|t - 1}} = {v_{t - 1|t - 1}} + a\Delta t\quad\quad(2)
\]
上面所述的位置
\(d\)
和速度
\(v\)
称为系统的状态变量，如果还需要估计其他的状态变量，如汽车的耗油量、发动机的温度等，都可以类似位置和速度一样加进来，这里仅以位置和速度为例介绍。上面两个状态变量的理想估计可以写成紧凑的矩阵形式：
\[{{\pmb{x}}_{t|t - 1}} = \left[ {\begin{array}{*{20}{c}}
1&{\Delta t}\\
\end{array}} \right] \times \left[ {\begin{array}{*{20}{c}}
{{d_{t - 1}}}\\
{{v_{t - 1}}}
\end{array}} \right] + {\pmb{B}}\left[ {\begin{array}{*{20}{c}}
{\frac{1}{2}\Delta {t^2}}\\
{\Delta t}
\end{array}} \right] \times {{\pmb{u}}_t} = {\pmb{F}}{{\pmb{x}}_{t - 1|t - 1}} + {\pmb{B}}{{\pmb{u}}_t}\quad\quad(3)\]
这里
\({\pmb{F}} = \left[ {\begin{array}{*{20}{c}}
1&{\Delta t}\\
\end{array}} \right]\)
称为状态变换矩阵，
\({\pmb{B}} = \left[ {\begin{array}{*{20}{c}}
{\frac{1}{2}\Delta {t^2}}\\
{\Delta t}
\end{array}} \right]\)
称为控制矩阵，
\({{\pmb{u}}_t} = a\)
称为控制向量。
这里
\({{\pmb{x}}_{t|t - 1}}\)
表示基于
\(t-1\)
时刻估计状态下理论预测的
\(t\)
时刻的状态，
\({{\pmb{x}}_{t - 1|t - 1}}\)
表示在
\(t-1\)
时刻综合理论预测和实际测量得到的估计值。其他的下标具有类似意思。
然而实际系统中总会存在噪声，比如汽车的运动并不严格遵循匀加速运动。那么汽车在
\(t\)
时刻的真实状态可以表示为基于
\(t-1\)
时刻的真实状态值经过
\(\Delta t\)
时间后加上噪声项：
\[{{\pmb{x}}_t} = {\pmb{F}}{{\pmb{x}}_{t - 1}} + {\pmb{B}}{{\pmb{u}}_t} + {{\pmb{w}}_t}\quad\quad(4)
\]
这里
\({\pmb{x}_{t}}\)
表示
\(t\)
时刻的状态真实值，
\({{\pmb{x}}_{t - 1}}\)
表示汽车在
\(t-1\)
时刻的状态真实值。噪声
\({{\pmb{w}}_t}\)
为服从均值为0，协方差为
\({{\pmb{Q}}_t}\)
的二元独立高斯分布，
\({{\pmb{Q}}_t} = {\mathop{\rm cov}} ({{\pmb{w}}_t})\)
。
那么如何度量理论估计值
\(\pmb{x}_{t|t - 1}\)
与真实状态
\({\pmb{x}_{t}}\)
之间的差距呢？也就是说理论估计值的估计误差有多少？这可以用协方差矩阵来表示：
\[{{\pmb{P}}_{t|t - 1}} = {\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}})\quad\quad(5)
\]
(5)式经过推导得：
\[\begin{array}{l}
{{\pmb{P}}_{t|t - 1}} = {\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}})\\
{\rm{ \quad\quad\:\:= cov(}}{\pmb{F}}{{\bf{x}}_{t - 1}} + {\pmb{B}}{{\pmb{u}}_t}{\rm{ + }}{{\pmb{w}}_t} - {\pmb{F}}{{\pmb{x}}_{t - 1|t - 1}} - {\pmb{B}}{{\pmb{u}}_t}{\rm{)}}\\
{\rm{ \quad\quad\:\:= cov}}\left( {{\pmb{F}}({{\pmb{x}}_{t - 1}} - {{\pmb{x}}_{t - 1|t - 1}}){\rm{ + }}{{\pmb{w}}_t}} \right)\\
\quad\quad\:\:= {\pmb{F}}{\mathop{\rm cov}} ({{\pmb{x}}_{t - 1}} - {{\pmb{x}}_{t - 1|t - 1}}){{\pmb{F}}^{\rm T}} + {\mathop{\rm cov}} ({{\pmb{w}}_t})\\
\quad\quad\:\:= {\pmb{F}}{{\pmb{P}}_{t - 1|t - 1}}{{\pmb{F}}^{\rm T}} + {{\pmb{Q}}_t}
\end{array}\quad\quad(6)\]
2.2 实际测量
第二种
是通过各种传感器或其他手段来测量状态变量，比如力传感器，加速度传感器等。同样的，通过传感器测量得到的准确值可以表示为：
\[{{\pmb{z}}_t} = {\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t}\quad\quad(7)
\]
这里
\(\pmb{H}\)
也是一个变换矩阵，作用是将真实状态空间映射到测量空间，比如说前面理论估计的是位置和速度，如果传感器测量的是力或者加速度，那么需要通过变换矩阵
\(\pmb{H}\)
将位置和加速度映射到力或加速度相同的空间中来。噪声
\({{\pmb{v}}_t}\)
为服从均值为0，协方差为
\({{\pmb{R}}_t}\)
的二元独立高斯分布，
\({{\pmb{R}}_t} = {\mathop{\rm cov}} ({{\pmb{v}}_t})\)
。
类似于理论预测中用协方差来度量理论估计值与真实值之间的估计误差，同样的，理论估计值与测量值之间的误差也可以用协方差矩阵来表示：
\[{{\pmb{S}}_t} = {\mathop{\rm cov}} ({{\pmb{z}}_t} - {\pmb{H}}{{\pmb{x}}_{t|t - 1}})\quad\quad(8)
\]
(8)式经过推导得：
\[\begin{array}{l}
{{\pmb{S}}_t} = {\mathop{\rm cov}} ({{\pmb{z}}_t} - {\pmb{H}}{{\pmb{x}}_{t|t - 1}})\\
\quad= {\mathop{\rm cov}} ({\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t} - {\pmb{H}}{{\bf{x}}_{t|t - 1}})\\
\quad= {\mathop{\rm cov}} \left( {{\pmb{H}}({{\pmb{x}}_t} - {{\pmb{x}}_{t|t - 1}}){{\pmb{H}}^{\rm T}}} \right) + {\mathop{\rm cov}} ({{\pmb{v}}_t})\\
\quad= {\pmb{H}}{{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}} + {{\pmb{R}}_t}
\end{array}\quad(9)\]
2.3 理论预测与实际测量融合
现在已经知道了
\(t\)
时刻的理论预测值和实际测量值，并知道了理论预测值与
\(t\)
时刻真实值及测量值之间的估计误差，那么根据理论预测值和实际测量值得到
\(t\)
时刻的估计值呢？卡尔曼滤波的思想是分别给理论预测值和实际测量值一个权重，通过理论预测值与实际测量值的加权线性组合来得到估计值，即：
\[{{\pmb{x}}_{t|t}} = {{\pmb{K}}_t}{{\pmb{z}}_t} + ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{x}}_{t|t - 1}}\quad(10)
\]
这里
\({{\pmb{K}}_t}\)
称为
卡尔曼增益
，
那么这个权重怎么确定呢？
我们的目标是使得
\(t\)
时刻加权后的估计值与系统的真实值之间的误差最小，也就是
\({{\pmb{x}}_{t|t}}\)
与
\({{\pmb{x}}_t}\)
之间的距离最小化。在这个前提下求得的权重因子（卡尔曼增益）就是最佳的。那么
\({{\pmb{x}}_{t|t}}\)
与
\({{\pmb{x}}_t}\)
之间的距离最小化可以描述为：
\[\mathop {\min }\limits_{{{\pmb{K}}_t}} {\left\| {{{\pmb{x}}_t} - {{\pmb{x}}_{t|t}}} \right\|^2} \Leftrightarrow \mathop {\min }\limits_{{{\pmb{K}}_t}} {\rm{Tr}}\left( {{\mathop{\rm cov}} ({{\pmb{x}}_t} - {{\pmb{x}}_{t|t}})} \right) = \mathop {\min }\limits_{{{\pmb{K}}_t}} {\mathop{\rm Tr}\nolimits} ({{\pmb{P}}_{t|t}})\quad(11)
\]
这一位置只需到展开
\({{\pmb{P}}_{t|t}}\)
，然后对其求一阶导，令导数等于0，即可得到卡尔曼增益：
\[{{\pmb{K}}_t} = {{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}}{\pmb{S}}_t^{ - 1}\quad\quad\quad(12)
\]
得到卡尔曼增益后，将
\({{\pmb{K}}_t}\)
代回
\({{\pmb{P}}_{t|t}}\)
中即可得到关于
\({{\pmb{K}}_t}\)
的加权估计下的协方差矩阵:
\[{{\pmb{P}}_{t|t}} = ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{P}}_{t|t - 1}}\quad\quad\quad(13)
\]
2.4 卡尔曼滤波器迭代过程
综上所述，卡尔曼滤波器的迭代过程可以总结为：
\[\begin{array}{l}
{{\pmb{x}}_{t|t - 1}} = {\pmb{F}}{{\bf{x}}_{t - 1|t - 1}} + {\pmb{B}}{{\pmb{u}}_t}\quad(1)\\
{{\pmb{P}}_{t|t - 1}} = {\pmb{F}}{{\pmb{P}}_{t - 1|t - 1}}{{\pmb{F}}^{\rm T}} + {{\bf{Q}}_t}\quad(2)\\
{{\pmb{z}}_t} = {\pmb{H}}{{\pmb{x}}_t} + {{\pmb{v}}_t}\quad(3)\\
{{\pmb{S}}_t} = {\pmb{H}}{{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}} + {{\pmb{R}}_t}\quad(4)\\
{{\pmb{x}}_{t|t}} = {{\pmb{K}}_t}{{\pmb{z}}_t} + ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{x}}_{t|t - 1}}\quad(5)\\
{{\pmb{K}}_t} = {{\pmb{P}}_{t|t - 1}}{{\pmb{H}}^{\rm T}}{\pmb{S}}_t^{ - 1}\quad(6)\\
{{\pmb{P}}_{t|t}} = ({\pmb{I}} - {{\pmb{K}}_t}{\pmb{H}}){{\pmb{P}}_{t|t - 1}}\quad(7)
\end{array}\]
三、讨论
最小二乘（Least Squares）是优化方法中的一种特殊情况，而卡尔曼滤波又是最小二乘法的一种特殊情况。 古典最小二乘中，假设了每一次测量的权重相同，但事实上这样并不合理，后来演化为加权最小二乘法，至此最小二乘估计所做的都是批处理（Batch），这样比较占内存，不符合动态系统状态估计的需要，即每一次更新输入时，都要从新计算之前所有的记录值。而后，提出递推最小二乘法，模型就不用每次都重新计算了。与递归最小二乘相似，卡尔曼滤波加入了系统内部变化的考虑。即利用process model对系统在下一时刻的状态进行预测。
当对于系统不够了解时，使用最小二乘法比较合适，而对于系统了解比较多时，可以采用Kalman滤波。改变量测噪声、系统噪声都会对Kalman滤波的效果产生影响，而不会对最小二乘滤波产生影响，而改变最小二乘的阶数会对其产生影响。
往期精选：
新基建新机遇！100页PPT
毫米波大规模阵列中的AOA估计
线性降维：主成分分析PCA原理分析与仿真验证
5G+AI：有哪些新的研究方向和新范式？
简述3D点云配准算法
5G为人工智能与工业互联网赋能|79页高清PPT
智能算法|以动物命名的算法
一份超全面的机器学习公共数据集
矩阵填充|奇异值阈值算法
可重构/大规模智能反射表面reconfigurable/large intelligent surface综述
迭代硬阈值类算法总结||IHT/NIHT/CGIHT/HTP
软阈值迭代算法（ISTA）和快速软阈值迭代算法（FISTA）
伍德伯里矩阵恒等式（Woodbury matrix identity）
压缩感知：一种新型亚采样技术
最大比率传输
更多精彩内容请关注订阅号
优化与算法
和加入QQ讨论群1032493483获取更多资料
