Kalman原理应用

介绍

• 卡尔曼滤波(Kalman filter)是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。它的提出者，鲁道夫.E.卡尔曼，在一次访问NASA埃姆斯研究中心时，发现这种方法能帮助解决阿波罗计划的轨道预测问题，后来NASA在阿波罗飞船的导航系统中确实也用到了这个滤波器。
• 只要是存在不确定信息的动态系统，卡尔曼滤波就可以对系统下一个状态做出推测。即便有噪声信息干扰，卡尔曼滤波通常也能很好工作。因此卡尔曼滤波非常适合不断变化的系统，它的优点还有内存占用较小(只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择。

原理

状态方程：$$x_k=A{x}_{k-1}+B{u}_k+w_k$$
观测方程：$$z_k=H{x}_k+v_k$$
过程噪声：$$w_k\in N(0;Q_k)$$
观测噪声(高斯白噪声)：$$v_k\in N(0;R_k)$$



假设小车以a为加速度，速度为v直线前进，某时刻小车的位置与速度为：
$$\begin{gathered} P_i=P_{i-1}+v_i\Delta t+\frac{1}{2}a\Delta t^2 \\ {v}_i=v_{i-1}+a\Delta t \end{gathered}$$

1.二维状态：

$$x_t=\left[\begin{array}{c}P\\ v\end{array}\right]$$
[Pv]=[1Δt01][Pi−1vi−1]+[Δt22Δt]ai\left[ \begin{matrix} P \\ v \end{matrix} \right]=\left[ \begin{matrix} 1 & \Delta t