【滤波跟踪】扩展卡尔曼滤波器 (EKF) GPS 数据滤波跟踪【含Matlab源码 2316期】

原创已于 2024-11-03 09:14:24 修改 · 1.1k 阅读

24 ·

CC 4.0 BY-SA版权

文章标签：

#matlab

于 2023-12-27 13:37:42 首次发布

Matlab信号处理（进阶版）专栏收录该内容

1589 篇文章

订阅专栏

本文介绍了扩展卡尔曼滤波(EKF)的基本原理，包括状态方程和量测方程的转换，以及在Matlab中的具体应用实例，展示了EKF在GPS定位中的效果并与最小二乘法进行对比。

💥💥💥💥💥💥💥💥💞💞💞💞💞💞💞💞💞Matlab武动乾坤博客之家💞💞💞💞💞💞💞💞💞💥💥💥💥💥💥💥💥
🚀🚀🚀🚀🚀🚀🚀🚀🚀🚀🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚤🚀🚀🚀🚀🚀🚀🚀🚀🚀🚀
在这里插入图片描述
🔊博主简介：985研究生，Matlab领域科研开发者；

🚅座右铭：行百里者，半于九十。

🏆代码获取方式：
优快云 Matlab武动乾坤—代码获取方式

更多Matlab信号处理仿真内容点击👇
①Matlab信号处理（进阶版）

⛳️关注优快云 Matlab武动乾坤，更多资源等你来！！

⛄一、EKF算法简介

扩展卡尔曼滤波是利用泰勒级数展开方法将非线性滤波问题转化成近似的线性滤波问题,利用线性滤波的理论求解非线性滤波问题的次优滤波算法。其系统的状态方程和量测方程分别如式(1)、式(2)所示:
在这里插入图片描述
式中,X(k)为n维的随机状态向量序列,Z(k)为n维的随机量测向量序列,f(k,x(k))为空气阻力,v(k)、w(k)为零均值的正态(高斯)白噪声序列,其方差分别满足:

协方差的一步预测为:

量测预测值为:

相应的协方差为:
在这里插入图片描述
增益为:

状态更新方程为:

协方差更新方程为:

式中,I为与协方差同维的单位矩阵。

二阶扩展卡尔曼滤波的泰勒展开保留到二阶项,其状态的一步预测为:
在这里插入图片描述
协方差的一步预测为:

量测预测值为:

协方差更新方程为:
在这里插入图片描述
式中,I为与协方差同维的单位矩阵。

⛄二、部分源代码

clear all
close all
clc

load SV_Pos % position of satellites
load SV_Rho % pseudorange of satellites

T = 1; % positioning interval
N = 25;% total number of steps
% State vector is as [x Vx y Vy z Vz b d].', i.e. the coordinate (x,y,z),
% the clock bias b, and their derivatives.

% Set f, see [1]
f = @(X) ConstantVelocity(X, T);

% Set Q, see [1]
Sf = 36;Sg = 0.01;sigma=5; %state transition variance
Qb = [SfT+SgTTT/3 SgTT/2;
SgTT/2 Sg*T];
Qxyz = sigma^2 * [T^3/3 T^2/2;
T^2/2 T];
Q = blkdiag(Qxyz,Qxyz,Qxyz,Qb);

% Set initial values of X and P
X = zeros(8,1);
X([1 3 5]) = [-2.168816181271560e+006
4.386648549091666e+006
4.077161596428751e+006]; %Initial position
X([2 4 6]) = [0 0 0]; %Initial velocity
X(7,1) = 3.575261153706439e+006; %Initial clock bias
X(8,1) = 4.549246345845814e+001; %Initial clock drift
P = eye(8)*10;

fprintf(‘GPS positioning using EKF started\n’)
tic

for ii = 1:N
% Set g
g = @(X) PseudorangeEquation(X, SV_Pos{ii}); % pseudorange equations for each satellites

% Set R
Rhoerror = 36;                                               % variance of measurement error(pseudorange error)
R = eye(size(SV_Pos{ii}, 1)) * Rhoerror; 

% Set Z
Z = SV_Rho{ii}.';                                            % measurement value

[X,P] = Extended_KF(f,g,Q,R,Z,X,P);
Pos_KF(:,ii) = X([1 3 5]).';                                 % positioning using Kalman Filter
Pos_LS(:,ii) = Rcv_Pos_Compute(SV_Pos{ii}, SV_Rho{ii});      % positioning using Least Square as a contrast

fprintf('KF time point %d in %d  ',ii,N)
time = toc;
remaintime = time * N / ii - time;
fprintf('Time elapsed: %f seconds, Time remaining: %f seconds\n',time,remaintime)

end

% Plot the results. Relative error is used (i.e. just subtract the mean)
% since we don’t have ground truth.
for ii = 1:3
subplot(3,1,ii)
plot(1:N, Pos_KF(ii,:) - mean(Pos_KF(ii,:)),‘-r’)
hold on;grid on;
plot(1:N, Pos_LS(ii,:) - mean(Pos_KF(ii,:)))
legend(‘EKF’,‘ILS’)
xlabel(‘Sampling index’)
ylabel(‘Error(meters)’)
end
ha = axes(‘Position’,[0 0 1 1],‘Xlim’,[0 1],‘Ylim’,[0 1],‘Box’,‘off’,‘Visible’,‘off’,‘Units’,‘normalized’, ‘clipping’ , ‘off’);
text(0.5, 1,‘\bf Relative positioning error in x,y and z directions’,‘HorizontalAlignment’,‘center’,‘VerticalAlignment’, ‘top’);