一维卡尔曼滤波及其C++代码实现——雷达追踪恒定速度的飞机

问题背景

用雷达测量飞机位置，如果雷达测量的方向有范围，那么需要估计飞机大致的位置，不然南辕北辙，可能根本测不到飞机。如果飞机匀速运动，那当然好预测。但是现实中的风阻等干扰肯定会使这种匀速的幻梦破灭。

因此，我们需要建立一种能兼容误差的模型来预测飞机的位置。

问题描述

图片里面是英文的，不感兴趣的小伙伴可以直接看下面的中文分析。

问题大意为有一架飞机在距雷达30000m的位置，以恒定速度400m/s远离雷达，且只在以雷达为端点的射线上运动，雷达每5s测一次飞机的位置，理论上飞机下一次位置应为30200，。但因为干扰，飞机实际位置为30171。

现在，给你飞机1到10的测量位置，需要你来建立模型，来使模型的估计值尽量接近飞机位置的测量值。

建立这样的模型肯定有很多方法，这里介绍的就是其中一种：卡尔曼滤波。

卡尔曼滤波考虑两种误差，一是雷达没测准，二是飞机速度改变了。

跳出这个例子，用更普遍性的语言说是考虑变量（路程） 与 变量的变化率（速度）没测准。

于是，卡尔曼滤波考虑两个误差函数 α 和 β ，并用在n时刻时分别预测n时刻与n+1时刻的值来更新接下来的预测值。

xn,n代表在n时预测n的位置xn+1,n代表在n时预测n+1的位置vn,n代表在n时预测n的速度vn+1,n代表在n时预测n+1的速度zn表示n时的位置测量值x_{n,n}代表在n时预测n的位置\\x_{n+1,n}代表在n时预测n+1的位置\\v_{n,n}代表在n时预测n的速度\\v_{n+1,n}代表在n时预测n+1的速度\\z_n表示n时的位置测量值xn,n​代表在n时预测n的位置xn+1,n​代表在n时预测n+1的位置vn,n​代表在n时预测n的速度v