✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍一、背景一无人机应用对 GPS 定位的依赖无人机在当今的各个领域如航拍测绘、物流配送、农业监测以及灾害救援等方面都有着广泛应用。准确的定位信息对于无人机执行任务至关重要而全球定位系统GPS是无人机获取位置信息的主要手段之一。例如在物流配送中无人机需要依据 GPS 提供的精确位置准确降落在指定地点完成货物投递在灾害救援时无人机依靠 GPS 定位快速抵达受灾区域进行搜索和救援工作。二GPS 测量延迟带来的挑战然而在实际应用中GPS 测量存在不可忽视的延迟问题。这一延迟可能由多种因素导致包括信号传播延迟、卫星信号处理时间、接收机内部处理延迟等。例如当无人机快速飞行并需要实时调整飞行路径以避开障碍物时GPS 测量延迟可能使无人机接收到的位置信息滞后于实际位置导致飞行控制决策失误增加碰撞风险。这种延迟严重影响了无人机定位的准确性和实时性进而对无人机的安全稳定飞行以及任务执行效果产生负面影响。三延迟卡尔曼滤波DKF的应用意义延迟卡尔曼滤波DKF作为一种专门处理测量延迟问题的滤波算法能够有效地应对 GPS 测量延迟带来的挑战。它通过对系统状态进行估计和预测利用延迟测量数据来修正估计结果从而提高无人机定位的精度和实时性。DKF 为解决无人机 GPS 测量延迟问题提供了一种有效的解决方案有助于提升无人机在复杂环境下的飞行性能和任务执行能力。二、原理一系统模型构建⛳️ 运行结果 部分代码classdef DelayedKalmanFilterMerwe handle%%properties (SetAccess private)% StatesxPQnmkappaalphalambdagammaXiWi% Delayed statesx_sP_sQ_sXi_sWi_sn_sm_saW_prea_imu_preR_biend%%properties (Constant)g 9.81;e3 [0, 0, 1];ge3 9.81*[0, 0, 1];I3 eye(3);Z3 zeros(3);end%%methods%%function E DelayedKalmanFilterMerwe()n 10;m 2*n 1;% alpha 1.0;alpha 1.45;kappa 10;E.n n;E.m m;E.kappa kappa;E.alpha alpha;E.lambda alpha^2 * (n kappa) - n;E.gamma sqrt(n E.lambda);E.x zeros(n, 1);E.Xi zeros(n, m);E.Wi zeros(m, 1);E.P zeros(n);E.n_s 2*n;E.m_s 2*E.n_s 1;E.x_s zeros(E.n_s, 1);E.P_s zeros(E.n_s);E.Xi_s zeros(E.n_s, E.m_s);E.Wi_s zeros(E.m_s, 1);E.R_bi eye(3);E.a [0, 0, 0];E.a_imu_pre [0, 0, 0];E.W_pre [0, 0, 0];end%% Predictionfunction prediction(E, h, a_imu, W_imu)E.sigma_points();Xk zeros(E.n, E.m);for i 1:E.mXk(:,i) f(E, E.Xi(:, i), h, a_imu, W_imu);endE.Xi Xk;[E.x, E.P] unscented_transformation(E, Xk, E.Wi, E.Q);end%% IMU correctionfunction correction_imu(E, ypr, V_R_imu)% z reshape(R, 9, 1);z ypr;R V_R_imu;Y zeros(3, E.m);for i 1:E.mY(:,i) E.Xi(7:9,i);end[y, Py] unscented_transformation(E, Y, E.Wi, R);Pxz zeros(E.n, 3);for i 1:E.mPxz Pxz E.Wi(i)*(E.Xi(:,i) - E.x)*(Y(:,i) - y);endK Pxz*inv(Py);E.x E.x K*(z - y);E.P E.P - K*Py*K;end%% GPS correctionfunction correction_gps(E, x_gps, v_gps, V_x_gps, V_v_gps)z [x_gps; v_gps];R blkdiag(V_x_gps, V_v_gps);Y zeros(6, E.m);for i 1:E.mY(:,i) E.Xi(1:6,i);end[y, Py] unscented_transformation(E, Y, E.Wi, R);Pxz zeros(E.n, 6);for i 1:E.mPxz Pxz E.Wi(i)*(E.Xi(:,i) - E.x)*(Y(:,i) - y);endK Pxz*inv(Py);E.x E.x K*(z - y);E.P E.P - K*Py*K;end%% Equation of motionfunction Xk f(E, X, h, a_imu, W_imu)xk X(1:3);vk X(4:6);ypr X(7:9);Rk eul2rotm(ypr);b_a_k X(10);R_pre Rk;b_a_pre b_a_k;W E.R_bi*W_imu;Rk Rk*expm_SO3(h/2*(W E.W_pre));% This assumes IMU provide acceleration without gak Rk*E.R_bi*a_imu b_a_k*E.e3;a_pre R_pre*E.R_bi*E.a_imu_pre b_a_pre*E.e3;xk xk h*vk h^2/2*a_pre;vk vk h/2*(ak a_pre);E.W_pre W;E.a_imu_pre a_imu;E.a ak;Xk [xk; vk; rotm2eul(Rk); b_a_k];end%% Delayed Predictionfunction delayed_prediction(E, h, a_imu, W_imu)E.delayed_sigma_points();Xk zeros(E.n_s, E.m_s);for i 1:E.m_sXk(:,i) delayed_f(E, E.Xi_s(:, i), h, a_imu, W_imu);endE.Xi_s Xk;[E.x_s, E.P_s] unscented_transformation(E, Xk, E.Wi_s, E.Q_s);E.x E.x_s(1:E.n,1);E.P E.P_s(1:E.n,1:E.n);end%% Delayed IMU correctionfunction delayed_correction_imu(E, ypr, V_R_imu)% z reshape(R, 9, 1);z ypr;R V_R_imu;Y zeros(3, E.m_s);for i 1:E.m_sY(:,i) E.Xi_s(7:9,i);end[y, Py] unscented_transformation(E, Y, E.Wi_s, R);Pxz zeros(E.n_s, 3);for i 1:E.m_sPxz Pxz E.Wi_s(i)*(E.Xi_s(:,i) - E.x_s)*(Y(:,i) - y);endK Pxz*inv(Py);E.x_s E.x_s K*(z - y);E.P_s E.P_s - K*Py*K;E.x E.x_s(1:E.n,1);E.P E.P_s(1:E.n,1:E.n);end%% Delayed GPS correctionfunction delayed_correction_gps(E, x_gps, v_gps, V_x_gps, V_v_gps)z [x_gps; v_gps];R blkdiag(V_x_gps, V_v_gps);Y zeros(6, E.m_s);for i 1:E.m_sY(:,i) E.Xi_s(11:16,i);end[y, Py] unscented_transformation(E, Y, E.Wi_s, R);Pxz E.P_s(1:10,11:16);K Pxz*inv(Py);E.x E.x_s(1:10) K*(z - y);E.P E.P_s(1:10,1:10) - K*Py*K;end%% Delayed equation of motionfunction Xk delayed_f(E, X, h, a_imu, W_imu)xk X(1:3);vk X(4:6);ypr real(X(7:9));Rk eul2rotm(ypr);b_a_k X(10);R_pre Rk;b_a_pre b_a_k;W E.R_bi*W_imu;Rk Rk*expm_SO3(h/2*(W E.W_pre));% This assumes IMU provide acceleration without gak Rk*E.R_bi*a_imu b_a_k*E.e3;a_pre R_pre*E.R_bi*E.a_imu_pre b_a_pre*E.e3;xk xk h*vk h^2/2*a_pre;vk vk h/2*(ak a_pre);% Save dataE.W_pre W;E.a_imu_pre a_imu;E.a ak;Xk [xk; vk; rotm2eul(Rk); b_a_k; X(11:20)];end%% Delayed sigma pointsfunction delayed_sigma_points(E)E.Xi_s(:,1) E.x_s;E.Wi_s(1) E.kappa / (E.n_s E.kappa);sqrt_P sqrtm(E.P_s);for i 1:E.n_sE.Xi_s(:,i1) E.x_s E.gamma*sqrt_P(:,i);E.Xi_s(:,iE.n_s1) E.x_s - E.gamma*sqrt_P(:,i);endfor i 2:E.m_sE.Wi_s(i) 1 / (2*(E.n_sE.kappa));E.Wi_s(iE.n_s1) 1 / (2*(E.n_s E.kappa));endend%% Augment the statefunction update_augmented_state(E)E.x_s [E.x; E.x];E.P_s [E.P, E.P;E.P, E.P];end%% Sigma pointsfunction sigma_points(E)E.Xi(:,1) E.x;E.Wi(1) E.kappa / (E.n E.kappa);sqrt_P sqrtm(E.P);for i 1:E.nE.Xi(:,i1) E.x E.gamma*sqrt_P(:,i);E.Xi(:,iE.n1) E.x - E.gamma*sqrt_P(:,i);endfor i 2:E.mE.Wi(i) 1 / (2*(E.nE.kappa));E.Wi(iE.n1) 1 / (2*(E.n E.kappa));endend%% Unscented transformationfunction [xUT, PUT] unscented_transformation(E, XUT, WUT, Noise)[mUT, nUT] size(XUT);xUT zeros(mUT, 1);for i 1:nUTxUT xUT WUT(i)*XUT(:,i);endPUT zeros(mUT, mUT);for i 1:nUTPUT PUT WUT(i)*(XUT(:,i) - xUT)*(XUT(:,i) - xUT);endPUT PUT Noise;end%% Update parametersfunction update_parameters(E, R_bi)E.R_bi R_bi;end%% Update initial valuesfunction update_init_values(E, P, Q)E.P P;E.Q Q;E.Q_s blkdiag(Q, Q);E.P_s [P, P;P, P];end%% Get statesfunction [x, v, R, P] output_states(E)x E.x(1:3);v E.x(4:6);R eul2rotm(real(E.x(7:9)));P E.P;endendend 参考文献往期回顾扫扫下方二维码