GPS从入门到放弃（二十五） --- 卡尔曼滤波

GPS从入门到放弃（二十五） — 卡尔曼滤波

概述

单点定位的结果因为是单独一个点一个点进行的，所以连续起来看数据可能出现上串下跳的情况，事实上并不符合实际情况。为了解决这个问题，考虑到物体运动的连续性和运动变化的缓慢性，可以通过滤波器来平滑位置轨迹。
滤波器的设计需要对物体的运动做一些理性的、常规的假设，比如要符合牛顿运动定律等。这里最常用的滤波器就是卡尔曼滤波器。

卡尔曼滤波器用来解决用线性微分方程描述的离散时间控制过程中的状态估计问题。其目标是使系统状态的估计值有最小均方误差。卡尔曼滤波器来源于匈牙利数学家卡尔曼的博士论文，推导过程这里就不多赘述了，在诸多文献中都有讲解，我们直接讲怎么用。

对于一个线性离散系统，设其系统方程为：
$${\mathbf{x}}_k=\mathbf{A}{\mathbf{x}}_{k-1}+\mathbf{B}{\mathbf{u}}_{k-1}+{\mathbf{w}}_{k-1}$$
测量方程式为：
$${\mathbf{y}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k$$
其中随机变量 ${\mathbf{w}}_k$ 和 ${\mathbf{v}}_k$ 分别表示过程噪声与测量噪声。假设它们是相互独立的正态分布的白噪声，且其协方差阵分别为 $\mathbf{Q}$ 和 $\mathbf{R}$。（要注意的是，实际应用中，$\mathbf{A}$，$\mathbf{B}$，$\mathbf{H}$，