基于EKF的航线轨迹预测（完整代码）

最新推荐文章于 2024-04-18 19:08:33 发布

大胆无敌

最新推荐文章于 2024-04-18 19:08:33 发布

阅读量1.5k

点赞数 13

文章标签：算法 matlab 信号处理开发语言

本文链接：https://blog.csdn.net/tycodehh/article/details/134766858

版权

前言

在许多路线追踪实例中，所采用的追踪算法大多是卡尔曼滤波器（KF）或者是扩展的卡尔曼滤波器（EKF）,亦或者是基于EKF的优化算法。因此，理解并掌握EKF的思路并熟练运用便成为了涉足追踪领域的基础，本文就是记录笔者在学习EKF后的一些感想及仿真实现。

一、EKF的来源

在信号处理中，我们总是假设一个变量遵循着一个确定的表达式进行随时间的变化，比如最简单的一个表达式之一

$x=2t$

这个式子就表示对于任意一个时刻t，x的值永远是t的2倍。但在实际生活中，这样理想的情况几乎是不存在的，总会有一些干扰项影响我们最终得到的x，那么我们现在便有了两种情况，一是理想的状态方程，如下(由矢量高斯马尔可夫方程简化而来)：

$s[n] = s[n-1] + u[n]$

二则是实际的观测方程：

$x[n] = s[n] + w[n]$

其中的u和w都表示对应的噪声，都是为了模拟真实情况而引入的，其中u应该尽可能小。我们的目的就是通过已知的观测量x去推导出对应的状态量s，因此我们所求得的s并不是理想状态的s，而只是它的一个估计量，这种估计方法我们便成为卡尔曼滤波（KF），估计值与理想值之间的偏差我们用最小均方误差（MSE）去衡量。

以上是一般的一阶高斯马尔可夫状态方程的情况，当状态方程变为多阶时，情况略有不同，但思路类似，依旧可以用KF的方法进行计算。但在实际中，我们经常面对的状态方程和观测方程都是非线性的，这个时候KF的方法就不太准确了，我们需要将其转换为线性方程再用KF的方法，这个转换过程就是一阶泰勒近似，所求得的矩阵（也可为向量）便称为雅可比矩阵，这种操作也让KF摇身一变为EKF，也就是扩展的卡尔曼滤波，这便是EKF来源的简单介绍。

二、EKF的算法流程

接下来就是如何建立观测值与估计值之间的联系了，鉴于推导过程异常复杂，这里就不展开解释了，只讲解大致思路。

首先我们有两个方程，含义见上文：

$s[n] = As[n-1] + Bu[n]$

$x[n] = H[n]s[n] + w[n]$

A和H均是对应的状态转移矩阵，其与s的乘积我们可以将其简化，写成s的函数，即a(s[n-1])和h(s[n])，此时的函数不一定是线性函数，而为了将其变为线性函数，我们需要用到泰勒展开并忽略高阶项，如下：

$a(s[n-1]) \approx a(\hat{s}[n-1]|n-1) + A[n-1](s[n-1]-\hat{s}[n-1]|n-1)$

$h(s[n]) \approx h(\hat{s}[n|n-1]) + H[n] (s[n]-\hat{s}[n|n-1])$

这样我们便换成了线性高斯马尔可夫状态方程的形式，再用KF的方法进行估计就可。也就是先估计状态量和MSE矩阵，得到卡尔曼增益，再根据观测量对用卡尔曼增益对估计的状态量和MSE进行修正，作为下一时刻的初始值，循环往复，这样便完成了整个预测流程。

三、仿真实现

完成EKF的建模后，我们代入具体的场景对其进行仿真测试。

我们这里假设有一个飞行物或者任何什么运动的物体，它本身遵循一定的状态方程在二维平面内进行运动，我们只能观测到他在二维平面中与原点的距离和对应的角度，根据这些观测量预测出他在二维平面内的运动轨迹，尽可能的减弱各种噪声的影响从而实现跟踪。

我们先设定物体的一些运动参数：

%% 参数设置


Vx = -0.2;
Vy = 0.2;
Ay = -0.005;
deltau = 0.01;
Ux = normrnd(0,deltau,[1,length(n)]);
Uy = normrnd(0,deltau,[1,length(n)]);
for t = 2:length(n)
    Vx(t) = Vx(t-1);
    Vy(t) = Vy(t-1) + Ay ;
end

画图即为：

接下来我们对该理想轨迹加入噪声即为我们的观测值：

for t = 2:length(n)
    rx(t) = rx(t-1) + Vx(1)*delta;
    ry(t) = ry(t-1) + Vy(t)*delta;
    R(t) = sqrt(rx(t)^2+ry(t)^2);

    beta(t) = atan(ry(t)/rx(t));

    Rhat(t) = R(t) + wR(t);
    betahat(t) = beta(t) + wbeta(t);

    if rx(t)<=0 && ry(t)<=0
    rx_hat(t) = -Rhat(t)*cos(betahat(t));
    ry_hat(t) = -Rhat(t)*sin(betahat(t));
    else if rx(t)>0 && ry(t)<=0
    rx_hat(t) = Rhat(t)*cos(betahat(t));
    ry_hat(t) = -Rhat(t)*sin(betahat(t));
        end
    rx_hat(t) = Rhat(t)*cos(betahat(t));
    ry_hat(t) = Rhat(t)*sin(betahat(t));
    end
end

画图即为：

可以看到加入噪声后轨迹明显扭曲了起来，虽然x、y坐标的尺度都比较小，但我们仍然希望能将轨迹进行去噪，使轨迹尽可能的平滑，于是我们便用这些观测值带入我们的EKF算法，获得轨迹的估计值：

%% 算法过程
for t= 2:length(n)
   s_temp(:,t) = A*s_hat(:,t-1);
   MM_temp =  A*MM*A.'+Q;
   
   rx_hat_t = s_temp(1,t);
   ry_hat_t = s_temp(2,t);
   R_temp = sqrt(rx_hat_t^2 + ry_hat_t^2);
   HH_temp = [rx_hat_t/R_temp,ry_hat_t/R_temp,0,0;
            -ry_hat_t/(R_temp^2),rx_hat_t/(R_temp^2),0,0];
        
   KK = MM_temp*HH_temp.'*inv(CC + HH_temp*MM_temp*HH_temp.');%KK:4x2

   h_temp = [R_temp;
             atan(ry_hat_t/rx_hat_t)];%h_temp:2x1
         
   s_hat(:,t) = s_temp(:,t)+KK*(xx(:,t)-h_temp);
   MM = (eye(4,4)-KK*HH_temp)*MM_temp;
end

画图即为：