【滤波跟踪】GPS跟踪的扩展Kalman滤波器实现matlab代码

✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍一、引言在全球定位系统GPS跟踪应用中准确地估计目标的位置、速度等状态信息至关重要。然而GPS 测量数据往往包含噪声且目标的运动模型通常具有非线性特性。扩展 Kalman 滤波器EKF作为一种经典的非线性滤波算法能够有效地处理这类问题通过对非线性系统进行线性化近似实现对目标状态的最优估计。二、GPS 跟踪问题描述⛳️ 运行结果 部分代码clear allload(GPSdata.mat)est NonLinearLeastSquares(gps_data,ref_data_struct);%plot3(ref_data_struct.traj_ned(1,:),ref_data_struct.traj_ned(2,:), ref_data_struct.traj_ned(3,:))%hold on%figure%plot(ref_data_struct.traj_ned(1,:),ref_data_struct.traj_ned(2,:))% plot3(est.x_h(1,:), est.x_h(2,:), est.x_h(3,:))% hold on% plot3(ref_data_struct.traj_ned(1,:),ref_data_struct.traj_ned(2,:), ref_data_struct.traj_ned(3,:))%plot(est.x_h(1,:), est.x_h(2,:))sat_with_val [1, 8, 10, 19, 20, 25, 26];conf[1,3,5,6,7];Nlength(gps_data(1).PseudoRange);M length(gps_data);y [];satpos [];for i 1:length(sat_with_val)y [y; gps_data(sat_with_val(i)).PseudoRange];satpos [satpos; gps_data(sat_with_val(i)).Satellite_Position_NED];endT ref_data_struct.Ts;c ref_data_struct.c;R_k ref_data_struct.s2r;k*G_k - K_k*R_ek*K_k;b(:,n)est_x(:,n1)P_k*H_k/R_ek*e_k;end%%plot(b(1,500:end), b(3,500:end));hold onplot(ref_data_struct.traj_ned(1,:),ref_data_struct.traj_ned(2,:))plot(est.x_h(1,:),est.x_h(2,:))hold off%plot3(est_x(1,:), est_x(3,:),est_x(5,:))%plot3(b(1,:), b(3,:),b(5,:))%plot3(ref_data_struct.traj_ned(1,:),ref_data_struct.traj_ned(2,:), ref_data_struct.traj_ned(3,:))%figure%plot(est_x(1,:), est_x(3,:))%% xfigure(2)plot(ref_data_struct.traj_ned(1,:),ref_data_struct.traj_ned(2,:))hold onplot(est.x_h(1,:),est.x_h(2,:))plot(b(1,500:end), b(3,500:end));legend(True,Non-linear LS,EKF estimation)title(The estimation of the vehicle position for large covariance)xlabel(X)ylabel(Y)zlabel(Z)hold offfigure (3)errorxref_data_struct.traj_ned(1,:)-est_x(1,1:2015);sqerrorxref_data_struct.traj_ned(1,:)-est.x_h(1,:);plot(errorx)hold onplot(sqerrorx)title(Errors of X axis)xlabel(index)ylabel(error)plot(ool(1,:))plot(oop(1,:))legend(EKF estimation,non-linear LS,3\sigma interval,3\sigma interval)hold offfigure (4)erroryref_data_struct.traj_ned(2,:)-est_x(3,1:2015);sqerroryref_data_struct.traj_ned(2,:)-est.x_h(2,:);plot(errory)hold onplot(sqerrory)title(Errors of Y axis)xlabel(index)ylabel(error)plot(ool(2,:))plot(oop(2,:))legend(EKF estimation,non-linear LS,3\sigma interval,3\sigma interval)hold offfigure (5)errorzref_data_struct.traj_ned(3,:)-est_x(5,1:2015);sqerrorzref_data_struct.traj_ned(3,:)-est.x_h(3,:);plot(errorz)hold onplot(sqerrorz)title(Errors of Z axis)xlabel(index)ylabel(error)plot(ool(3,:))plot(oop(3,:))legend(EKF estimation,non-linear LS,3\sigma interval,3\sigma interval)hold offfigure (6)errortref_data_struct.x_clk(1,:).*c-est_x(6,1:2015);sqerrortref_data_struct.x_clk(1,:).*c-est.x_h(4,:);plot(errort)title(Errors of clock offset (meter))xlabel(index)ylabel(error)hold onplot(sqerrort)plot(ool(4,:))plot(oop(4,:))legend(EKF estimation,non-linear LS,3\sigma interval,3\sigma interval)hold offfigure (7)errort2ref_data_struct.x_clk(2,:).*c-est_x(7,1:2015);plot(errort2)title(Errors of clock drift(meter))xlabel(index)ylabel(error)hold onplot(ool(5,:))plot(oop(5,:))legend(EKF estimation,3\sigma interval,3\sigma interval)hold off 参考文献更多免费数学建模和仿真教程关注领取