一、卡尔曼滤波算法1.1 一维卡尔曼滤波算法卡尔曼滤波算法是一种最优线性状态估计算法是一种离散的算法分为预测与观测两部分。在运行滤波器之前需要设置观测噪声和过程噪声这决定了滤波器更相信观测值或更相信预测值。首先通过被控对象的数学特性、当前系统输入以及之前得到的数据预测现在的数据。然后通过获取被控对象数据的观测器观测现在的数据更新得到计算卡尔曼增益。最后滤波得到的数值即为相当于预测值与观测值的加权平均数。公式如下首先初始化观测噪声、过程噪声、预测可信度、滤波器初始值。在第1个时间步长后获取此时状态量观测值和输入量观测值进行第1次卡尔曼滤波........................................................................在第k个时间步长后获取此时状态量观测值和输入量观测值进行第k次卡尔曼滤波1.2 拓展卡尔曼滤波算法拓展卡尔曼滤波算法将维度拓展至多维此时在预测、更新时需要考虑矩阵中存在对角线元素以外的元素。协方差便用于量化估计的不确定性。对于一个向量计算得到其均值为协方差矩阵定义为只是理论是一种线性运算方式根据具体情况具体调整需要注意当存在线性变换时有在引入协方差概念后1.1中部分变量定义为观测噪声协方差矩阵、过程噪声协方差矩阵、预测值协方差矩阵、状态转移矩阵。在卡尔曼滤波算法中存在通过上一步数值预测当前数值和通过测量数值更新滤波数值两部分。在预测部分中算法使用类似的方式进行预测因此在预测过程中对当前的预测值协方差矩阵的计算过程中需要采用类似的方式计算。公式如下对于更新卡尔曼滤波器的算法需要引入概念新息。新息用于表示预测值与测量值之差的协方差设观测矩阵为新息为即则新息的协方差矩阵为注意到用于新息协方差矩阵运算的矩阵是。公式如下二、用于四元数的拓展卡尔曼滤波算法已经得到四元数在连续时间下的状态转移矩阵需要将上式转化为适用于卡尔曼滤波器的离散时间的状态转移矩阵。假设则有。求解该微分方程解为。对进行泰勒展开保留两位即为对于卡尔曼滤波器需要通过时刻的得到时刻的则将上式转化为式中为滤波器工作周期。三、MATLAB仿真3.1 飞行器真实姿态设置设置飞行器2s的飞行姿态为俯仰角按三角函数变化一个周期从0°到10°到0°模拟抬头后恢复的过程%% 1. 时间设置与真实姿态ZYX 欧拉角顺序 t 0:0.01:2; % 时间向量02秒步长0.01s N length(t); % 总采样点数 T 0.01; % 采样周期 phi_true zeros(1, N); % 真实滚转角roll单位弧度 theta_true deg2rad(10 * (1 - cos(pi * t))); % 真实俯仰角pitch动态变化 psi_true zeros(1, N); % 真实偏航角yaw保持为0 % 计算欧拉角的时间导数用于生成真实角速度 dphi_dt zeros(1, N); dtheta_dt deg2rad(10 * pi * sin(pi * t)); % 对 theta_true 求导 dpsi_dt zeros(1, N);通过飞行器姿态求出飞行器角速度%% 2. 根据真实姿态计算机体真实角速度 [p; q; r]FRD 体轴系 gyro_true zeros(3, N); % 真实角速度矩阵3×N for k 1:N phi phi_true(k); theta theta_true(k); % 构建从欧拉角速率到机体角速度的转换矩阵 T(phi, theta)ZYX顺序 T_mat [ ... 1, 0, -sin(theta); ... 0, cos(phi), sin(phi)*cos(theta); ... 0, -sin(phi), cos(phi)*cos(theta) ... ]; euler_rates [dphi_dt(k); dtheta_dt(k); dpsi_dt(k)]; % 欧拉角速率向量 gyro_true(:,k) T_mat * euler_rates; % 转换为机体角速度 end3.2 模拟传感器使用陀螺仪、加速度计、磁力计获取飞行器真实姿态需要设置传感器噪声%% 3. 模拟 IMU 传感器测量值带高斯噪声 rng(1); % 设置随机种子保证结果可复现 gyro_noise_std deg2rad(0.5); % 陀螺仪噪声标准差0.5 度/秒 acc_noise_std 0.05; % 加速度计噪声标准差m/s² mag_noise_std 0.02; % 磁力计噪声标准差归一化后 gravity 9.8; % 重力加速度大小m/s² mag_earth [1; 0; 0]; % 地磁场方向北向在 NED 坐标系中 % 初始化测量矩阵 gyro_meas zeros(3, N); % 陀螺仪测量值 acc_meas zeros(3, N); % 加速度计测量值 mag_meas zeros(3, N); % 磁力计测量值 for k 1:N % 当前真实姿态角 phi phi_true(k); theta theta_true(k); psi psi_true(k); % 计算旋转矩阵 R_be从 NED 地理坐标系 到 FRD 机体坐标系ZYX 欧拉角 RX [ 1 0 0 ; 0 cos(phi) sin(phi) ; 0 -sin(phi) cos(phi) ;]; RY [ cos(theta) 0 -sin(theta); 0 1 0 ; sin(theta) 0 cos(theta);]; RZ [ cos(psi) sin(psi) 0 ; -sin(psi) cos(psi) 0 ; 0 0 1 ;]; R_be RX * RY * RZ; % 陀螺仪测量 真实角速度 高斯噪声 gyro_meas(:,k) gyro_true(:,k) gyro_noise_std * randn(3,1); % ✅ 加速度计测量模型修正 % 在 FRD 坐标系中z 轴向下与重力同向 % 静止时加速度计应输出 [0; 0; g] % 因此acc R_be * g_ned其中 g_ned [0; 0; g] g_ned [0; 0; gravity]; acc_body R_be * g_ned; % 重力在机体坐标系中的投影 acc_meas(:,k) acc_body acc_noise_std * randn(3,1); % 磁力计测量 地磁场在机体坐标系中的投影 噪声并归一化 mag_body R_be * mag_earth; mag_body mag_body / norm(mag_body); % 归一化地磁场 mag_meas(:,k) mag_body mag_noise_std * randn(3,1); mag_meas(:,k) mag_meas(:,k) / norm(mag_meas(:,k)); % 再次归一化 end得到不采用滤波器时传感器得到的数据%% 4. 直接由加速度计和磁力计解算姿态无滤波 phi_direct zeros(1, N); % 直接解算的滚转角 theta_direct zeros(1, N); % 直接解算的俯仰角 psi_direct zeros(1, N); % 直接解算的偏航角 for k 1:N acc acc_meas(:, k); mag mag_meas(:, k); % ✅ 修正加速度计直接给出重力方向z 向下为正 % 因此重力单位向量 acc / norm(acc) g_body acc / norm(acc); % 根据重力向量解算 roll 和 pitchFRD 坐标系 % 推导关系g_body [-sinθ, sinφ·cosθ, cosφ·cosθ]^T phi_direct(k) atan2(g_body(2), g_body(3)); % 滚转角 φ atan2(gy, gz) theta_direct(k) -asin(max(-1, min(1, g_body(1)))); % 俯仰角 θ -asin(gx)注意负号 % 使用磁力计解算偏航角需进行倾角补偿 phi phi_direct(k); theta theta_direct(k); cphi cos(phi); sphi sin(phi); cth cos(theta); sth sin(theta); % 将磁力计数据投影到水平面 mag_hx mag(1)*cth mag(2)*sth*sphi mag(3)*sth*cphi; % 北向分量 mag_hy mag(2)*cphi - mag(3)*sphi; % 东向分量 psi_direct(k) atan2(-mag_hy, mag_hx); % 偏航角 ψ注意符号 end3.3 EKF算法初始化EKF%% 5. EKF 初始化 x_hat [1; 0; 0; 0]; % 初始四元数 [w; x; y; z] P diag([0.01, 0.01, 0.01, 0.01]); % 初始协方差矩阵 Q (T^2) * (gyro_noise_std^2) * eye(4); % 过程噪声协方差 R diag([0.01, 0.01, 0.01, 0.01]); % 观测噪声协方差 q_est zeros(4, N); % 存储 EKF 估计的四元数 q_est(:,1) x_hat; % 初始值EKF滤波%% 6. EKF 主循环融合陀螺仪、加速度计、磁力计 for k 2:N % 获取上一时刻的陀螺仪测量值用于状态预测 p gyro_meas(1, k-1); q_val gyro_meas(2, k-1); r gyro_meas(3, k-1); % 构建状态转移矩阵基于四元数微分方程 F [1, -0.5*T*p, -0.5*T*q_val, -0.5*T*r; 0.5*T*p, 1, 0.5*T*r, -0.5*T*q_val; 0.5*T*q_val, -0.5*T*r, 1, 0.5*T*p; 0.5*T*r, 0.5*T*q_val, -0.5*T*p, 1]; % 预测步骤 x_pred F * x_hat; x_pred x_pred / norm(x_pred); % 单位化四元数 P_pred F * P * F Q; % 预测协方差 % 观测更新由当前 acc 和 mag 解算观测四元数 [q0_obs, q1_obs, q2_obs, q3_obs] get_Qua_FRD_Corrected(mag_meas(:,k), acc_meas(:,k)); z_obs [q0_obs; q1_obs; q2_obs; q3_obs]; z_obs z_obs / norm(z_obs); % 单位化 % 观测矩阵线性化此处简化为单位阵 H eye(4); S H * P_pred * H R; % 新息协方差 K P_pred * H / S; % 卡尔曼增益 % 处理四元数符号歧义确保内积为正 y z_obs - x_pred; if dot(x_pred, z_obs) 0 y -z_obs - x_pred; end % 更新步骤 x_hat x_pred K * y; x_hat x_hat / norm(x_hat); % 单位化 P (eye(4) - K*H) * P_pred; % 更新协方差 % 保存结果 q_est(:,k) x_hat; end %% % 子函数根据加速度计和磁力计数据计算四元数修正版适用于 FRD 坐标系 % 输入 % mag: 磁力计测量值3×1 向量已归一化 % acc: 加速度计测量值3×1 向量单位 m/s² % 输出 % q0,q1,q2,q3: 四元数 [w,x,y,z] function [q0, q1, q2, q3] get_Qua_FRD_Corrected(mag, acc) acc acc(:); mag mag(:); % ✅ 关键修正加速度计直接反映重力方向z 向下为正 % 因此重力单位向量 acc / norm(acc) g_body acc / norm(acc); % 从重力向量解算 roll 和 pitch phi atan2(g_body(2), g_body(3)); % 滚转角 theta -asin(max(-1, min(1, g_body(1)))); % 俯仰角注意负号 % 倾角补偿后的磁力计解算 yaw cphi cos(phi); sphi sin(phi); cth cos(theta); sth sin(theta); mag_hx mag(1)*cth mag(2)*sth*sphi mag(3)*sth*cphi; % 北向分量 mag_hy mag(2)*cphi - mag(3)*sphi; % 东向分量 psi atan2(-mag_hy, mag_hx); % 偏航角 % 将 ZYX 欧拉角转换为四元数 cr cos(phi/2); sr sin(phi/2); cp cos(theta/2); sp sin(theta/2); cy cos(psi/2); sy sin(psi/2); q0 cr*cp*cy sr*sp*sy; q1 sr*cp*cy - cr*sp*sy; q2 cr*sp*cy sr*cp*sy; q3 cr*cp*sy - sr*sp*cy; end四元数转化欧拉角%% 7. 将 EKF 估计的四元数转换为欧拉角ZYX 顺序 phi_est zeros(1, N); theta_est zeros(1, N); psi_est zeros(1, N); for i 1:N q0 q_est(1,i); q1 q_est(2,i); q2 q_est(3,i); q3 q_est(4,i); % 四元数转 ZYX 欧拉角标准公式 phi_est(i) atan2(2*(q0*q1 q2*q3), 1 - 2*(q1^2 q2^2)); theta_est(i) asin(max(-1, min(1, 2*(q0*q2 - q3*q1)))); % 防止数值溢出 psi_est(i) atan2(2*(q0*q3 q1*q2), 1 - 2*(q2^2 q3^2)); end3.4 显示曲线将真实姿态、未滤波传感器信息、滤波后传感器信息绘图%% 8. 绘图真实值 vs 直接测量 vs EKF 滤波 figure(Position, [100, 100, 900, 700]); subplot(3,1,1); plot(t, rad2deg(phi_true), r, LineWidth, 2); hold on; plot(t, rad2deg(phi_direct), g--, LineWidth, 1.5); plot(t, rad2deg(phi_est), b:, LineWidth, 1.5); ylabel(滚转角 \phi (度)); grid on; legend(真实值, 直接测量加速度计, EKF 滤波, Location, best); subplot(3,1,2); plot(t, rad2deg(theta_true), r, LineWidth, 2); hold on; plot(t, rad2deg(theta_direct), g--, LineWidth, 1.5); plot(t, rad2deg(theta_est), b:, LineWidth, 1.5); ylabel(俯仰角 \theta (度)); grid on; legend(真实值, 直接测量加速度计, EKF 滤波, Location, best); subplot(3,1,3); plot(t, rad2deg(psi_true), r, LineWidth, 2); hold on; plot(t, rad2deg(psi_direct), g--, LineWidth, 1.5); plot(t, rad2deg(psi_est), b:, LineWidth, 1.5); xlabel(时间 (秒)); ylabel(偏航角 \psi (度)); grid on; legend(真实值, 直接测量磁力计, EKF 滤波, Location, best); sgtitle(姿态估计对比FRD 坐标系z 轴与重力同向 → 加速度计 z 分量为正);结果如下
