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

问题背景

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

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

问题描述

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

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

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

---

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

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

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

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

$$\begin{gathered} 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时的位置测量值 \end{gathered}$$

n时刻预测n的位置：

我们设计的模型应该让α的值象征着距离测量到底有多准，因此考虑如下等式：

$x_{n,n}=x_{n,n-1}+\alpha \times (z_n-x_{n,n-1})$

α乘测量距离与上一次预测的值的差值，就相当于对预测进行了一次状态更新，来使得后面的预测更符合测量值。

n时刻预测n的速度：

同理可得：

$v_{n,n}=v_{n,n-1}+\beta (\frac{z_n-x_{n,n}}{t})$

---

当然，由上面的分析可知，α与β的取值是由我们给定的。
如果你有足够多的事实依据能直接找到α与β的较优解，那当然很好。

在更多没有信息的情况则需要我们一个个取值找，这个时候编个代码会方便很多。由于没有特殊的算法讲解，我就直接把带注释的代码放在下方，感兴趣的小伙伴自己消化吧。

#include <iostream>
using namespace std;

double ansa,ansb;//alpha and beta  

int z[1000];//测量值 

int x[1000][2];//v[x][0]是vnn,v[x][1]是vn+1n 
double v[1000][2];

int n,t;

void kal(double a, double b)
{


for (int i = 1; i <= n; i++)
{
//估计当前值 
x[i][0] = x[i-1][1] + a * (z[i] - x[i-1][1]);
v[i][0] = v[i-1][1] + b * ( (z[i] - x[i-1][1]) / t);

//预测未来值
x[i][1] = x[i][0] + v[i][0] * t;
v[i][1] = v[i][0];

//输出
cout<<x[i][0]<<" "<<v[i][0]<<endl;

}

return;
}


int main()
{
v[0][1] = 40.0, x[0][1] = 30000 , z[0] = 30000;

cin>>n>>t;

for (int i = 1;i <= n;i++)
{
cin>>z[i];
}

cin>>ansa>>ansb;

kal(ansa,ansb);

return 0;
}
/*
样例：
10 5 30171 30353 30756 30799 31018 31278 31276 31379 31748 32175 0.2 0.1

*/

原文地址：https://blog.csdn.net/qq_44635637/article/details/142696736

免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！