淘宝一个关键词要刷多久,江西seo推广方案,网站编辑岗位,wordpress英文版切换中文文章目录程序介绍系统建模滤波框架仿真设置性能对比代码优点运行结果MATLAB源代码程序介绍
本程序实现了 三维状态量的扩展卡尔曼滤波#xff08;EKF#xff09;组合导航仿真#xff0c;采用严格的15维误差状态模型#xff0c;状态向量包括#xff1a; x[pxpypzvxvyvzϕθ… 文章目录程序介绍系统建模滤波框架仿真设置性能对比代码优点运行结果MATLAB源代码程序介绍
本程序实现了 三维状态量的扩展卡尔曼滤波EKF组合导航仿真采用严格的15维误差状态模型状态向量包括
x[pxpypzvxvyvzϕθψbgxbgybgzbaxbaybaz]Tx \begin{bmatrix} p_x p_y p_z v_x v_y v_z \phi \theta \psi b_g^x b_g^y b_g^z b_a^x b_a^y b_a^z \end{bmatrix}^T x[pxpypzvxvyvzϕθψbgxbgybgzbaxbaybaz]T
其中位置 ppp、速度 vvv、姿态欧拉角 (ϕ,θ,ψ)(\phi, \theta, \psi)(ϕ,θ,ψ)、陀螺偏差 bgb_gbg、加速度计偏差 bab_aba 构成完整的15维误差状态。
系统建模
过程模型 由IMU观测驱动采用离散状态转移函数
pk1pkvkΔtvk1vk(Cbn(fm−ba)−g)ΔtΘk1Θk(ωm−bg)Δtbg,ba视为随机游走模型\begin{aligned} p_{k1} p_k v_k \Delta t \\ v_{k1} v_k \big(C_{bn}(f_m - b_a) - g\big)\Delta t \\ \Theta_{k1} \Theta_k (\omega_m - b_g)\Delta t \\ b_g, b_a \ \text{视为随机游走模型} \end{aligned} pk1vk1Θk1bg,bapkvkΔtvk(Cbn(fm−ba)−g)ΔtΘk(ωm−bg)Δt 视为随机游走模型
其中fm,ωmf_m, \omega_mfm,ωm 分别为测得的加速度与角速度CbnC_{bn}Cbn 为姿态方向余弦矩阵。
观测模型 来自GNSS观测量为位置和速度
滤波框架
扩展卡尔曼滤波按以下步骤实现
预测
x^k∣k−1f(x^k−1,uk−1)\hat{x}_{k|k-1} f(\hat{x}_{k-1}, u_{k-1}) x^k∣k−1f(x^k−1,uk−1)
Pk∣k−1FPk−1FTQP_{k|k-1} F P_{k-1} F^T Q Pk∣k−1FPk−1FTQ
更新 若有GNSS观测
KkPk∣k−1HT(HPk∣k−1HTR)−1K_k P_{k|k-1} H^T (H P_{k|k-1} H^T R)^{-1} KkPk∣k−1HT(HPk∣k−1HTR)−1
x^kx^k∣k−1Kk(zk−h(x^k∣k−1))\hat{x}_{k} \hat{x}_{k|k-1} K_k(z_k - h(\hat{x}_{k|k-1})) x^kx^k∣k−1Kk(zk−h(x^k∣k−1))
Pk(I−KkH)Pk∣k−1P_k (I - K_k H) P_{k|k-1} Pk(I−KkH)Pk∣k−1
其中FFF 和 HHH 分别为系统雅可比矩阵和观测雅可比矩阵。
仿真设置
真实轨迹为 螺旋上升运动即圆周运动叠加线性爬升。IMU数据由真实运动加噪声和随机偏差生成。GNSS每秒输出一次位置与速度观测。
性能对比
程序对比了三种轨迹
真实轨迹蓝线纯IMU积分结果红线EKF融合结果黑线 并绘制位置、速度、姿态曲线及误差随时间的变化计算均方根误差 (RMSE) 作为性能指标。
代码优点
使用严格的 15维误差状态建模保证了惯导/GNSS组合导航的一致性推导。包含 状态转移雅可比矩阵 和 观测雅可比矩阵 的显式计算便于理论分析与扩展。程序结果直观展示了 EKF在三维导航中的精度改进有效抑制纯IMU积分的发散显著降低位置与速度误差。
运行结果
三维轨迹对比 各轴速度、位置、姿态对比
误差对比
MATLAB源代码
部分代码
% 三维状态量的EKF例程严格的组合导航推导
% 基于15维误差状态模型位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)
% 作者matlabfilter
% 2025-08-25/Ver1 clear; clc; close all;
rng(0); % 固定随机种子%% 系统参数设置
dt 0.1; % 采样时间间隔 (s)
total_time 100; % 总仿真时间 (s)
N total_time / dt; % 采样点数%% 噪声参数设置
% IMU噪声参数
gyro_noise_std 0.01 * pi/180; % 陀螺噪声标准差 (rad/s)
accel_noise_std 0.001; % 加速度计噪声标准差 (m/s^2)
gyro_bias_std 0.001 * pi/180; % 陀螺偏差标准差 (rad/s)
accel_bias_std 0.0001; % 加速度计偏差标准差 (m/s^2)% GNSS观测噪声
gnss_pos_noise_std 3; % GNSS位置噪声标准差 (m)
gnss_vel_noise_std 0.1; % GNSS速度噪声标准差 (m/s)%% 过程噪声协方差矩阵Q (15×15)
% 状态顺序[位置(3), 速度(3), 姿态(3), 陀螺偏差(3), 加速度计偏差(3)]
Q zeros(15, 15);
% 位置噪声通过速度积分产生
Q(1:3, 1:3) eye(3) * (accel_noise_std * dt^2 / 2)^2;
% 速度噪声
Q(4:6, 4:6) eye(3) * (accel_noise_std * dt)^2;
% 姿态噪声
Q(7:9, 7:9) eye(3) * (gyro_noise_std * dt)^2;
% 陀螺偏差噪声
Q(10:12, 10:12) eye(3) * (gyro_bias_std * dt)^2;
% 加速度计偏差噪声
Q(13:15, 13:15) eye(3) * (accel_bias_std * dt)^2;
完整代码
如需帮助或有导航、定位滤波相关的代码定制需求请点击下方卡片联系作者
