✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信。
🍎个人主页:Matlab科研工作室
🍊个人信条:格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
旋翼无人机具有结构紧凑、机动性强、操作灵活等特点,可机载多种设备实现垂直起降、空中悬停等功能,广泛应用于军事、农业、影视等各个领域。但实现旋翼无人机的平稳飞行,需要复杂的传感器、快速的控制器和执行器以及复杂的导航控制算法作为支撑。而位姿信息就像无人机的"眼睛",是无人机实现自主飞行的基础。作为多传感器数据融合系统,INS/GPS组合导航系统可以得到无人机的高精度导航信息.本文提出了一种基于卡尔曼滤波器的自适应数据融合方法,并着重研究了在GPS/IMU组合导航中的应用.根据子系统的新息序列构造各传感器的环境信息,将卡尔曼滤波器调整到最优状态,从而提高组合导航系统的精度.仿真结果证明这种方法提高了数据融合的可靠性和精度.
⛄ 部分代码
classdef InEKF < handle
properties
mu; % Pose Mean
Sigma; % Pose Sigma
mu_pred; % Mean after prediction step
Sigma_pred; % Sigma after prediction step
end
methods
function obj = InEKF(init)
obj.mu = init.mu;
obj.Sigma = init.Sigma;
end
function prediction(obj, u)
g = [0; 0; 9.81];
dt = 1 / 10;
R_k = obj.mu(1:3, 1:3);
v_k = obj.mu(1:3, 4);
p_k = obj.mu(1:3, 5);
a_k = u(1:3);
omega_k = u(4:6);
R_pred = R_k * expm(skew(omega_k * dt));
v_pred = v_k + (R_k * Gamma_1(omega_k * dt) * a_k' + g) *dt;
p_pred = p_k + v_k * dt + 0.5 * (2 * R_k * Gamma_2(omega_k * dt) * a_k' + g) * dt ^2;
H_pred = [R_pred, v_pred, p_pred;
zeros(1,3), 1, 0;
zeros(1,3), 0, 1];
obj.propagation(H_pred, a_k, omega_k);
end
function propagation(obj, H_pred, a_k, omega_k)
dt = 1 / 10;
obj.mu_pred = H_pred;
% log linear
A = zeros(9);
A(1:3, 1:3) = - skew(omega_k);
A(4:6, 1:3) = - skew(a_k);
A(4:6, 4:6) = - skew(omega_k);
A(7:9, 4:6) = eye(3);
A(7:9, 7:9) = -skew(omega_k);
phi = expm(A * dt);
obj.Sigma_pred = obj.Sigma + phi * eye(9) * phi';
end
function correction(obj, gps_measurement)
b = [0; 0; 0; 0; 1];
H = [zeros(3), zeros(3), eye(3)];
% N just a covariance, so instead of doing the covariance stuff, just a 3 by 3
N = eye(3).*0.5;
Y = [gps_measurement'; 0; 1];
nu = obj.mu_pred \ Y - b;
S = H * obj.Sigma_pred * H' + N;
K = obj.Sigma_pred * H' * (S \ eye(size(S)));
% calculate zai
zai = K * nu(1:3);
zai_hat = zeros(5);
phi = zai(1:3);
rho1 = zai(4:6);
rho2 = zai(7:9);
jacobian_phi = eye(3);
theta = norm(phi);
jacobian_phi = jacobian_phi + (1 - cos(theta)) / theta^2 * skew(phi) ...
+ (theta - sin(theta)) / theta^3 * (skew(phi)^2);
zai_hat(1:3, 1:3) = expm(skew(phi));
zai_hat(1:3, 4) = jacobian_phi * rho1;
zai_hat(1:3, 5) = jacobian_phi * rho2;
zai_hat(4:5, 4:5) = eye(2);
obj.mu = obj.mu_pred * zai_hat;
obj.Sigma = (eye(9) - K * H) * obj.Sigma_pred * (eye(9) - K * H)' + K * N * K';
end
end
end
⛄ 运行结果
⛄ 参考文献
[1] 马培圣. 基于MEMS的微小型GPS/SINS组合测姿系统研究[D]. 南京航空航天大学.
[2] 刘洪剑. 旋翼无人机的姿态测量与组合导航算法研究[D]. 湖南大学, 2018.
[3] 毋建宏. 基于多传感器Kalman滤波器的GPS/IMU数据融合算法设计[J]. 微电子学与计算机, 2005, 22(6):4.
[4] 唐付林, 卫浩, 徐泽文,等. 基于迭代扩展卡尔曼滤波器的里程计算方法及装置:, CN115342829A[P]. 2022.
[5] 毋建宏, 张洪才. 基于环境信息的自适应卡尔曼滤波器在GPS/IMU组合导航中的应用[J]. 计算机应用研究, 2009, 26(10):3.
⛳️ 代码获取关注我
❤️部分理论引用网络文献,若有侵权联系博主删除
❤️ 关注我领取海量matlab电子书和数学建模资料