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

海神之光

已于 2024-10-13 14:25:13 修改

阅读量1.4k

点赞数 1

CC 4.0 BY-SA版权

分类专栏： Matlab信号处理（进阶版）文章标签： matlab

于 2023-01-14 22:40:06 首次发布

代码事宜私信博主

本文链接：https://blog.youkuaiyun.com/TIQCmatlab/article/details/128690288

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

1392 篇文章

订阅专栏

文章介绍了扩展卡尔曼滤波(EKF)的基本原理，它是一种处理非线性滤波问题的方法。通过泰勒级数展开，EKF将非线性系统转化为近似的线性系统进行处理。文中提供了部分MATLAB代码示例，用于演示EKF如何应用于GPS定位，比较了EKF与最小二乘法(LeastSquare)的定位效果。实验结果显示了EKF在不同时间点的定位误差，强调了其在实时定位中的作用。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

💥💥💥💥💥💥💞💞💞💞💞💞💞💞欢迎来到海神之光博客之家💞💞💞💞💞💞💞💞💥💥💥💥💥💥
在这里插入图片描述
✅博主简介：热爱科研的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’);