基础卡尔曼滤波怎么做（卡尔曼滤波 实现）

原则

卡尔曼滤波器是一种基本的预测定位算法。原理非常简单易懂。核心流程可以用图来说明：

本质上就是这两个状态过程的迭代，逐步准确定位。预测：当前状态环境下下一个时间段t的位置估计的计算值。更新：传感器获得更准确的位置信息，然后更新当前预测位置，即修正预测误差。你可能会问为什么传感器数据需要更新？由于现实世界中的传感器存在大量的噪声干扰，传感器数据不能完全可信。卡尔曼算法依赖于线性计算和高斯分布。我们用一维定位来介绍算法的实现。

接下来我们开始更新。经过预测，我们得到了传感器数据，这表明传感器发现的汽车当前位置应该是在位置26。这样的话，我们肯定感觉传感器的准确度比我之前的猜测要好。这是准确的。因此，方差自然会更小。最后我们觉得，真实的汽车位置应该更接近传感器数据，方差会更小，这样，想一想，就很清楚了。我猜测了一个预测值，现在有专家告诉我，有了相对可靠的数据，我对汽车位置的信心肯定会增加。经过这个时间段t后小车位置的最终更新就是下面的红色高斯图：

通过这样不断的移动和更新，汽车的位置最终会变得越来越准确。

一维模型下的卡尔曼公式：

预测

更新

参考代码：

#include #include #include 使用命名空间std;double new_mean, new_var;tuplemeasurement_update(double Mean1, double var1, double Mean2, double var2){ new_mean=(var2 * Mean1 + var1 * Mean2)/(var1 + var2); new_var=1/(1/var1 + 1/var2); return make_tuple(new_mean, new_var);}元组state_prediction(double Mean1, double var1, double Mean2, double var2){ new_mean=Mean1 + Mean2; new_var=var1 + var2; return make_tuple(new_mean, new_var);}int main(){ //测量值和测量方差double测量值[5]={ 5, 6, 7, 9, 10 };双测量_sig=4; //运动和运动方差double sport[5]={ 1, 1, 2, 1, 1 };双motion_sig=2； //初始状态double mu=0;双信号=1000； for (int i=0; i sizeof(measurements)/sizeof (measurements[0]); i++) { tie(mu, sig)=measurement_update(mu, sig,measurements[i],measurement_sig); printf('update: [%f, %f]', mu, sig); tie（mu，sig）=state_prediction（mu，sig，motion[i]，motion_sig）； printf('预测： [%f, %f]', mu, sig); } return 0;} 审稿人：李茜