简介针对GNSS-INS惯性导航系统的科研教学与课程设计需求这份Matlab仿真资源基于2021a环境采用扩展卡尔曼滤波EKF实现GNSS与INS的传感器融合完整呈现组合导航系统从建模到解算的流程。资源共22个文件包含13个m脚本、1个mat数据文件、1个avi操作录屏以及若干说明图片脚本负责核心算法与可视化录屏可逐步演示仿真操作数据文件提供实测训练输入整个压缩包仅4.52MB轻量易部署。目前已有610人学习适合本科、研究生阶段进行组合导航、卡尔曼滤波及惯性导航方向的教研与自学。通过对照录屏独立运行m文件学习者可快速复现EKF融合过程理解GNSS/INS松组合架构、状态方程构建与滤波参数调优思路获得一套可扩展的导航算法实验基底。1. 惯性导航系统的漂移问题正好是EKF要补的那块短板纯惯导几秒钟就会让你怀疑传感器坏了陀螺零偏让姿态慢慢转动加速度计积分让速度随时间划走。GNSS却完全相反绝对位置准确但输出频率低、城区还有遮挡。GNSS-INS融合就是把这两种互补信号用EKF按协方差最优地拧在一起在MATLAB仿真里先验证算法再上车。适合谁准备做组合导航、车辆定位或无人系统状态估计的工程师以及被IMU零偏和噪声折磨到想放弃的漂移问题研究生。这套仿真做的不是拖几个模块搭模型而是把EKF的状态方程、观测方程和离散化全部自己写一遍这样调试发散时才知道问题出在哪一环。2. EKF融合的核心误差状态模型、状态转移矩阵F与协方差Q的离散化2.1 为什么选误差状态而不是全量状态EKF在GNSS-INS里几乎都做在误差状态上。原因是惯性导航的机械编排是非线性方程直接对全量状态做EKF每次预测都要重新算雅可比矩阵而且姿态、速度和位置的数值量级差距很大一个0.001弧度的姿态残差就能对应几十米的位置变化协方差矩阵容易病态数值稳定性差。误差状态把标称轨迹和误差分离标称部分用惯导机械编排传播误差部分用线性化的EKF去估计。更新的时候把估计出的误差修正回标称状态然后把误差状态清零。这是松耦合GNSS-INS最常见的工程结构MATLAB仿真也按这个套路来。后面所有代码都围绕这个结构展开先想清楚这一步调参才不会乱。2.2 15维状态向量与连续时间F矩阵误差状态一般取15维维度分配如下表分量符号维数物理含义姿态误差δθ3NED系下的小角度姿态误差速度误差δv3NED系下的速度误差位置误差δp3NED系下的位置误差陀螺零偏误差δb_g3陀螺常值零偏估计误差加速度计零偏误差δb_a3加速度计常值零偏估计误差连续时间误差状态方程写作δx_dot F * δx G * w其中状态向量排列为[δθ; δv; δp; δb_g; δb_a]。对战术级仿真忽略地球自转和科里奥利项后F矩阵可以简化为下面这个形式用MATLAB构造最直观F zeros(15,15); % 姿态误差方程: δθ_dot -[omega×]*δθ - δb_g F(1:3,1:3) -skew(omega); % omega为NED系下的角速度 F(1:3,10:12) -eye(3); % 陀螺零偏误差影响姿态 % 速度误差方程: δv_dot -[f×]*δθ - δb_a F(4:6,1:3) -skew(f); % f为NED系下的比力 F(4:6,13:15) -eye(3); % 加计零偏误差影响速度 % 位置误差方程: δp_dot δv F(7:9,4:6) eye(3); % 零偏建模为随机游走F中对应位置为0代码里skew()是求向量反对称矩阵的辅助函数可以用[0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]直接写。omega取陀螺当前输出减去零偏估计f取加速度计当前输出减去零偏估计。这里把零偏导数设为零实际零偏缓慢变化的部分交给Q矩阵里的随机游走噪声吸收。2.3 离散化零阶保持与前向欧拉、后向欧拉的工程取舍连续时间F矩阵不能直接用于EKF预测必须离散化到IMU采样周期。常见做法有三种前向欧拉F_d I F*dt、后向欧拉F_d (I - F*dt)^-1、零阶保持ZOHF_d expm(F*dt)。前向欧拉实现最简单dt不超过0.01秒时精度足够后向欧拉无条件稳定但要算矩阵求逆ZOH用MATLAB的expm函数一步到位适合车辆动力学这种动态变化较快的场景。function [F_d, Q_d] discretizeEKF(F, G, Qc, dt, method) % method: euler 或 zoh if strcmpi(method, zoh) F_d expm(F * dt); % Q离散化的常见近似保留F_d的交叉项 Q_d F_d * (G * Qc * G) * F_d * dt; else F_d eye(size(F)) F * dt; A eye(size(F)) F * dt; Q_d A * (G * Qc * G) * A * dt; end endQc是连续时间的噪声功率谱密度矩阵单位分别是(rad/s)^2/Hz、(m/s^2)^2/Hz和零偏随机游走的(rad/s)^2/Hz形式。用前向欧拉时注意dt不能太大否则协方差会因高阶项截断而慢慢变负当IMU频率低于50Hz时我一般直接换ZOH避免后向欧拉矩阵求逆引入的额外数值误差。提示离散化这一步新手最常犯的错是直接用F*P*F Qc*dt而漏掉G矩阵导致过程噪声维度对不上。写成G*Qc*G*dt只是一个粗略近似上面代码里的对称形式更接近真实离散协方差建议直接采用。3. 在MATLAB里搭建松耦合EKFIMU/GNSS数据生成与滤波主循环3.1 先生成真值轨迹再反推IMU量测仿真IMU数据最可靠的方式不是随便造一组数组而是先定义真值运动轨迹再从轨迹反推理想IMU测量最后叠加零偏和噪声。这样做的好处是误差分析有真值做参照EKF调参看得到真实误差。下面是一段圆形轨迹生成代码水平面运动NED系下忽略重力项后加速度计理想输出直接等于运动加速度clear; clc; dt 0.01; % IMU采样周期100Hz t 0:dt:100; % 仿真时长100秒 N length(t); R 100; omg 0.1; % 圆半径100m角速度0.1rad/s % 真值轨迹NED系 pos_ned [R*sin(omg*t); R*(1-cos(omg*t)); zeros(1,N)]; vel_ned [R*omg*cos(omg*t); R*omg*sin(omg*t); zeros(1,N)]; acc_ned [-R*omg^2*sin(omg*t); R*omg^2*cos(omg*t); zeros(1,N)]; % 反推理想IMU量测水平运动时比力加速度 yaw_true omg*t; gyro_ideal [zeros(2,N); omg*ones(1,N)]; % 只有z轴有角速度 accel_ideal acc_ned; % 叠加零偏与白噪声 gyro_bias [0.5; -0.4; 0.3] * pi/180; % 常值零偏单位rad/s accel_bias [0.02; -0.03; 0.01]; % 常值零偏单位m/s^2 sigma_g 0.01*pi/180; % 陀螺白噪声 sigma_a 0.02; % 加计白噪声 gyro_noise sigma_g * sqrt(1/dt) * randn(3,N); accel_noise sigma_a * sqrt(1/dt) * randn(3,N); gyro gyro_ideal gyro_bias gyro_noise; accel accel_ideal accel_bias accel_noise;噪声为什么要乘sqrt(1/dt)连续白噪声功率谱密度到离散采样的转换关系离散噪声方差等于功率谱密度除以采样间隔。如果只想验证算法收敛性直接用sigma*randn也能跑但换成这种写法后Q矩阵里的数值可以直接从器件手册功率谱密度填写不用调来调去。3.2 GNSS观测模拟与坐标转换GNSS在真实环境中输出经纬高而EKF状态里存的是NED系位置。仿真里为了贴近实际情况会把真值NED位置转成经纬度再做一次正变换回来。这个来回转换能顺便检验坐标函数的正确性避免算法移植到实车数据时才发现坐标符号反了。function [x_ned, y_ned, z_ned] geodetic2ned(lat, lon, h, lat0, lon0, h0) % 简化坐标转换适合小范围仿真单位米 R_N 6378137; % WGS84长半轴 dLat (lat - lat0) * pi/180; dLon (lon - lon0) * pi/180; x_ned dLon * R_N * cos(lat0*pi/180); y_ned dLat * R_N; z_ned -(h - h0); end反转换写成ned2geodetic在MATLAB里直接按上式逆运算。生成GNSS观测时每隔1秒在真值位置上加高斯噪声gnss_rate 1; % GNSS更新频率1Hz sigma_gps [0.8; 0.8; 1.5]; % 水平0.8m垂直1.5m gps_idx 1; for k 1:gnss_rate*dt:... % 实际在主循环中判断 z_gps pos_ned(:,k) sigma_gps .* randn(3,1); endR矩阵由sigma_gps平方构成。注意GNSS垂直精度通常比水平差R矩阵里对应位置应给更大的方差否则EKF会把垂直方向的位置修得太激进反而放大速度误差。3.3 滤波主循环机械编排与协方差传播EKF主循环分三步标称状态机械编排预测、误差协方差传播、GNSS观测更新。标称状态里用四元数存姿态避免欧拉角奇异。% 初始化状态与协方差 state.quat [1;0;0;0]; state.vel zeros(3,1); state.pos pos_ned(:,1); state.gyro_bias zeros(3,1); state.accel_bias zeros(3,1); P blkdiag(eye(3)*0.01, eye(3)*0.1, eye(3)*0.1, eye(3)*1e-4, eye(3)*1e-4); for k 2:N dT t(k) - t(k-1); % 预测 [state, F, G, Qc] predictNominal(state, gyro(:,k), accel(:,k), dT); [F_d, Q_d] discretizeEKF(F, G, Qc, dT, euler); P F_d * P * F_d Q_d; P 0.5*(P P); % 强制对称防止数值误差破坏对称性 % GNSS更新 if mod(k, round(1/(gnss_rate*dt))) 0 z_gps pos_ned(:,k) sigma_gps .* randn(3,1); [state, P] updateEKF(state, P, z_gps, diag(sigma_gps.^2)); end % 记录位置误差、速度误差、零偏估计用于后处理 endpredictNominal函数里做四元数姿态更新和速度位置积分同时把F矩阵算出来。注意速度更新时NED系的重力向量是[0;0;9.8]加速度计输出减去零偏后加上重力再乘dtfunction [state, F, G, Qc] predictNominal(state, gyro, accel, dt) % 四元数小角度更新 dtheta gyro * dt; dq [1; 0.5*dtheta]; % 一阶近似忽略二阶小量 state.quat quatMultiply(state.quat, dq); state.quat state.quat / norm(state.quat); % 速度与位置更新 f accel - state.accel_bias; state.vel state.vel (f [0;0;9.8]) * dt; state.pos state.pos state.vel * dt; % 误差状态矩阵与上一章相同 F zeros(15,15); F(1:3,1:3) -skew(gyro); F(1:3,10:12) -eye(3); F(4:6,1:3) -skew(accel); F(4:6,13:15) -eye(3); F(7:9,4:6) eye(3); G zeros(15,12); G(1:3,1:3) -eye(3); G(4:6,4:6) -eye(3); G(10:12,7:9) eye(3); G(13:15,10:12) eye(3); Qc blkdiag(sigma_g^2*eye(3), sigma_a^2*eye(3), 1e-8*eye(3), 1e-6*eye(3)); endquatMultiply是四元数乘法MATLAB的航空航天工具箱里有现成函数没有工具箱就手写四元数乘法公式。skew用上一章提到的反对称矩阵构造。F矩阵中的gyro和accel直接取当前测量值因为误差状态线性化点的变化对结果影响很小工程上普遍这么处理。3.4 更新步观测矩阵H与卡尔曼增益GNSS位置观测直接对应误差状态中的位置误差所以观测矩阵是[0 0 I 0 0]。新息是GNSS位置减去标称状态位置修正量通过卡尔曼增益映射到15维误差状态function [state, P] updateEKF(state, P, z, R) H zeros(3,15); H(:,7:9) eye(3); % 位置观测矩阵 r z - state.pos; % 新息 S H * P * H R; K P * H / S; % 卡尔曼增益 dx K * r; % 误差状态估计 % 修正标称状态 state.pos state.pos dx(7:9); state.vel state.vel dx(4:6); state.gyro_bias state.gyro_bias dx(10:12); state.accel_bias state.accel_bias dx(13:15); % 姿态误差修正回四元数 dq [1; 0.5*dx(1:3)]; state.quat quatMultiply(state.quat, dq); state.quat state.quat / norm(state.quat); % 协方差更新标准形式 P (eye(15) - K * H) * P; P 0.5*(P P); end这里有几个参数容易看迷糊。K的维度是15x3r是3x1所以dx是15x1每个分量的顺序必须和状态定义一致。更新完成后误差状态理论上是零均值不需要把dx存进状态再清空直接修正标称状态即可。如果GNSS的数据率提高到10HzR里的方差应该保持真实GNSS精度不变而不是因为更新频率变高而缩小。4. 调参、跑通与结果验证Q/R初值、发散抑制与误差曲线判读4.1 先让仿真不崩Q和R的单位与初值EKF调参的第一步不是调增益而是把每个噪声参数的物理单位和量级写对。下面这张表是仿真初始值的常见参考范围单位很重要参数符号初值建议单位陀螺白噪声sigma_g0.01~0.1deg/s换算rad/s加速度计白噪声sigma_a0.01~0.1m/s^2陀螺零偏随机游走sigma_bg1e-4~1e-3deg/s^2加计零偏随机游走sigma_ba1e-5~1e-4m/s^3GNSS水平噪声sigma_gps_h0.5~2mGNSS垂直噪声sigma_gps_v1~3mQ矩阵里的随机游走项如果给太大零偏估计会跟着噪声乱跳位置误差反而变大给太小常值零偏要几百秒才能收敛。一个快速判断方法跑完后把零偏估计画出来如果曲线像白噪声一样剧烈抖动说明sigma_bg和sigma_ba大了一个数量级如果曲线缓慢偏离真值且长时间不回来说明Q偏小或R偏大。sigma_g 0.01*pi/180; % rad/s sigma_a 0.02; % m/s^2 sigma_bg 0.001*pi/180; % rad/s^2 sigma_ba 0.0001; % m/s^3 R_gps diag(sigma_gps.^2);这里sigma_bg的随机游走是rad/s^2不是rad/s很多仿真把单位搞错后零偏估计完全失真。零偏随机游走的物理含义是零偏变化率的功率谱密度换算时要用sigma_bg^2填进Qc对应块。4.2 R矩阵与GNSS更新率松耦合的节奏感松耦合下GNSS更新率通常只有1HzIMU是100Hz两次GNSS观测之间EKF要预测100次。R矩阵设得太小每1秒一次的修正会把位置拉得很猛速度曲线出现锯齿R设得太大GNSS信息被忽略轨迹退回纯惯导漂移。实际调参时先用GNSS标称精度设R再通过新息序列检验是否匹配。新息序列r的理论协方差是S H*P*H R因此构建卡方检验量% 在updateEKF里同时返回新息r和S chi2 r * (H*P*H R)^-1 * r; % 3自由度卡方阈值 threshold chi2inv(0.95, 3); if chi2 threshold % 新息超限可以考虑增大R或检查地图匹配异常 end这里chi2inv来自MATLAB统计工具箱。如果连续多个历元超限多半不是R参数问题而是状态已经发散或者GNSS观测出现了粗差在真实系统里应切换鲁棒更新策略而不是盲目调参。4.3 Q和R之外的发散抑制协方差限幅与零偏可观测性EKF发散最常见原因是协方差P不断缩小增益K趋近于零后续观测不再有效修正状态。这种情况在MATLAB仿真里表现为误差曲线前期收敛某时刻突然直线漂走且不再回来。一个简单有效的抑制手段是在每个周期对P的对角线设置上下限P max(P, Pmin); % 防止协方差过小 P min(P, Pmax); % 防止协方差爆炸Pmin和Pmax需要按状态变量分别给比如姿态误差给[1e-6, 1e-2]位置误差给[1e-3, 1e4]。这不是理论最优但工程上非常实用尤其可以防止长时间直线运动后水平位置协方差被压缩到无法恢复。还有一个必须注意的点零偏可观测性依赖运动激励。直线匀速运动时陀螺零偏和加速度计零偏几乎不可观测EKF只能靠过程噪声维持状态绕圈或加减速时才能把零偏估计出来。所以调零偏收敛速度时应设计包含匀速段和转弯段的轨迹否则无论Q怎么调都收敛不了。4.4 误差曲线怎么判读跑完仿真后画三张图位置误差、速度误差、陀螺与加计零偏估计。位置误差应在一个GNSS噪声标准差范围内波动且不能有单向增长趋势速度误差曲线应平滑不应出现每次GNSS更新时的尖刺零偏估计曲线应从初值缓慢收敛到预设真值附近。如果位置误差包络持续增大且呈现抛物线形态说明IMU数据里的常值零偏没有被估计出来检查EKF更新步的H矩阵是否放错了位置。如果速度误差出现周期性锯齿说明R矩阵与实际观测噪声不匹配而不是滤波算法写错。提示仿真初期前100个历元的误差尖峰不必在意那是初始协方差偏大导致的收敛过程。重点看100秒末段的稳态误差和零偏是否稳定。5. 更接近实车的玩法紧耦合、零偏在线估计与数据后处理5.1 把松耦合升级成紧耦合松耦合用GNSS的位置结果做观测信息量已经被前端RTK或定位引擎消化过一遍。紧耦合直接用伪距观测状态里要加一个接收机钟差项观测方程变成卫星伪距 几何距离 钟差 - 误差状态对位置的投影。MATLAB仿真里可以先从真值位置和卫星星历反推各通道伪距再在EKF里对每颗卫星单独建立H矩阵这样GNSS卫星少于4颗时系统仍能工作这是松耦合做不到的。紧耦合代码量比松耦合大不少但H矩阵的构造思路仍然是误差状态对观测的偏导核心框架不需要重写。5.2 零偏在线估计的精细操作零偏估计这块工程上有个通用技巧先跑一遍包含强转弯的轨迹看零偏估计是否收敛到同一组值然后把零偏从状态向量里去掉对比水平位置误差。如果去掉零偏后误差没有明显变大说明你的IMU质量好到不需要在线估计零偏直接在预测步里扣除常值补偿即可。反过来如果位置误差变化很大则必须保留零偏项。MATLAB仿真时可以用这种方式量化零偏在线估计的价值给方案设计提供数据支撑。5.3 把CSV数据接进MATLAB仿真的两个关键点用真实采集的IMU/GNSS数据做仿真时CSV导入是绕不开的第一步。要用detectImportOptions和readtimetable避免重复手写格式解析同时注意时间戳对齐opts detectImportOptions(imu.csv); tbl readtimetable(imu.csv, opts); imu_t seconds(tbl.t - tbl.t(1)); imu_data table2array(tbl(:, {gx,gy,gz,ax,ay,az})); % GNSS数据按时间戳插值到IMU时间轴 gps_t seconds(gps_tbl.t - gps_tbl.t(1)); gps_pos_interp interp1(gps_t, gps_pos, imu_t, linear, extrap);第一个关键点是时间戳必须归一化到秒且从0开始否则interp1会因为起始时间偏移产生错位第二个关键点是IMU与GNSS时间基准不一致时先做时间对齐再做坐标转换不要在坐标转换后再插值因为经纬度插值会引入非线性误差。这段对齐代码可以直接接到EKF主循环前面替换掉仿真生成的GNSS观测。本文还有配套的精品资源点击获取 
长河君 · 2026/9/16 17:41:04 · 阅读 1.2k MATLAB实现GNSS-INS组合导航：EKF误差状态模型与仿真详解
简介针对GNSS-INS惯性导航系统的科研教学与课程设计需求这份Matlab仿真资源基于2021a环境采用扩展卡尔曼滤波EKF实现GNSS与INS的传感器融合完整呈现组合导航系统从建模到解算的流程。资源共22个文件包含13个m脚本、1个mat数据文件、1个avi操作录屏以及若干说明图片脚本负责核心算法与可视化录屏可逐步演示仿真操作数据文件提供实测训练输入整个压缩包仅4.52MB轻量易部署。目前已有610人学习适合本科、研究生阶段进行组合导航、卡尔曼滤波及惯性导航方向的教研与自学。通过对照录屏独立运行m文件学习者可快速复现EKF融合过程理解GNSS/INS松组合架构、状态方程构建与滤波参数调优思路获得一套可扩展的导航算法实验基底。1. 惯性导航系统的漂移问题正好是EKF要补的那块短板纯惯导几秒钟就会让你怀疑传感器坏了陀螺零偏让姿态慢慢转动加速度计积分让速度随时间划走。GNSS却完全相反绝对位置准确但输出频率低、城区还有遮挡。GNSS-INS融合就是把这两种互补信号用EKF按协方差最优地拧在一起在MATLAB仿真里先验证算法再上车。适合谁准备做组合导航、车辆定位或无人系统状态估计的工程师以及被IMU零偏和噪声折磨到想放弃的漂移问题研究生。这套仿真做的不是拖几个模块搭模型而是把EKF的状态方程、观测方程和离散化全部自己写一遍这样调试发散时才知道问题出在哪一环。2. EKF融合的核心误差状态模型、状态转移矩阵F与协方差Q的离散化2.1 为什么选误差状态而不是全量状态EKF在GNSS-INS里几乎都做在误差状态上。原因是惯性导航的机械编排是非线性方程直接对全量状态做EKF每次预测都要重新算雅可比矩阵而且姿态、速度和位置的数值量级差距很大一个0.001弧度的姿态残差就能对应几十米的位置变化协方差矩阵容易病态数值稳定性差。误差状态把标称轨迹和误差分离标称部分用惯导机械编排传播误差部分用线性化的EKF去估计。更新的时候把估计出的误差修正回标称状态然后把误差状态清零。这是松耦合GNSS-INS最常见的工程结构MATLAB仿真也按这个套路来。后面所有代码都围绕这个结构展开先想清楚这一步调参才不会乱。2.2 15维状态向量与连续时间F矩阵误差状态一般取15维维度分配如下表分量符号维数物理含义姿态误差δθ3NED系下的小角度姿态误差速度误差δv3NED系下的速度误差位置误差δp3NED系下的位置误差陀螺零偏误差δb_g3陀螺常值零偏估计误差加速度计零偏误差δb_a3加速度计常值零偏估计误差连续时间误差状态方程写作δx_dot F * δx G * w其中状态向量排列为[δθ; δv; δp; δb_g; δb_a]。对战术级仿真忽略地球自转和科里奥利项后F矩阵可以简化为下面这个形式用MATLAB构造最直观F zeros(15,15); % 姿态误差方程: δθ_dot -[omega×]*δθ - δb_g F(1:3,1:3) -skew(omega); % omega为NED系下的角速度 F(1:3,10:12) -eye(3); % 陀螺零偏误差影响姿态 % 速度误差方程: δv_dot -[f×]*δθ - δb_a F(4:6,1:3) -skew(f); % f为NED系下的比力 F(4:6,13:15) -eye(3); % 加计零偏误差影响速度 % 位置误差方程: δp_dot δv F(7:9,4:6) eye(3); % 零偏建模为随机游走F中对应位置为0代码里skew()是求向量反对称矩阵的辅助函数可以用[0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0]直接写。omega取陀螺当前输出减去零偏估计f取加速度计当前输出减去零偏估计。这里把零偏导数设为零实际零偏缓慢变化的部分交给Q矩阵里的随机游走噪声吸收。2.3 离散化零阶保持与前向欧拉、后向欧拉的工程取舍连续时间F矩阵不能直接用于EKF预测必须离散化到IMU采样周期。常见做法有三种前向欧拉F_d I F*dt、后向欧拉F_d (I - F*dt)^-1、零阶保持ZOHF_d expm(F*dt)。前向欧拉实现最简单dt不超过0.01秒时精度足够后向欧拉无条件稳定但要算矩阵求逆ZOH用MATLAB的expm函数一步到位适合车辆动力学这种动态变化较快的场景。function [F_d, Q_d] discretizeEKF(F, G, Qc, dt, method) % method: euler 或 zoh if strcmpi(method, zoh) F_d expm(F * dt); % Q离散化的常见近似保留F_d的交叉项 Q_d F_d * (G * Qc * G) * F_d * dt; else F_d eye(size(F)) F * dt; A eye(size(F)) F * dt; Q_d A * (G * Qc * G) * A * dt; end endQc是连续时间的噪声功率谱密度矩阵单位分别是(rad/s)^2/Hz、(m/s^2)^2/Hz和零偏随机游走的(rad/s)^2/Hz形式。用前向欧拉时注意dt不能太大否则协方差会因高阶项截断而慢慢变负当IMU频率低于50Hz时我一般直接换ZOH避免后向欧拉矩阵求逆引入的额外数值误差。提示离散化这一步新手最常犯的错是直接用F*P*F Qc*dt而漏掉G矩阵导致过程噪声维度对不上。写成G*Qc*G*dt只是一个粗略近似上面代码里的对称形式更接近真实离散协方差建议直接采用。3. 在MATLAB里搭建松耦合EKFIMU/GNSS数据生成与滤波主循环3.1 先生成真值轨迹再反推IMU量测仿真IMU数据最可靠的方式不是随便造一组数组而是先定义真值运动轨迹再从轨迹反推理想IMU测量最后叠加零偏和噪声。这样做的好处是误差分析有真值做参照EKF调参看得到真实误差。下面是一段圆形轨迹生成代码水平面运动NED系下忽略重力项后加速度计理想输出直接等于运动加速度clear; clc; dt 0.01; % IMU采样周期100Hz t 0:dt:100; % 仿真时长100秒 N length(t); R 100; omg 0.1; % 圆半径100m角速度0.1rad/s % 真值轨迹NED系 pos_ned [R*sin(omg*t); R*(1-cos(omg*t)); zeros(1,N)]; vel_ned [R*omg*cos(omg*t); R*omg*sin(omg*t); zeros(1,N)]; acc_ned [-R*omg^2*sin(omg*t); R*omg^2*cos(omg*t); zeros(1,N)]; % 反推理想IMU量测水平运动时比力加速度 yaw_true omg*t; gyro_ideal [zeros(2,N); omg*ones(1,N)]; % 只有z轴有角速度 accel_ideal acc_ned; % 叠加零偏与白噪声 gyro_bias [0.5; -0.4; 0.3] * pi/180; % 常值零偏单位rad/s accel_bias [0.02; -0.03; 0.01]; % 常值零偏单位m/s^2 sigma_g 0.01*pi/180; % 陀螺白噪声 sigma_a 0.02; % 加计白噪声 gyro_noise sigma_g * sqrt(1/dt) * randn(3,N); accel_noise sigma_a * sqrt(1/dt) * randn(3,N); gyro gyro_ideal gyro_bias gyro_noise; accel accel_ideal accel_bias accel_noise;噪声为什么要乘sqrt(1/dt)连续白噪声功率谱密度到离散采样的转换关系离散噪声方差等于功率谱密度除以采样间隔。如果只想验证算法收敛性直接用sigma*randn也能跑但换成这种写法后Q矩阵里的数值可以直接从器件手册功率谱密度填写不用调来调去。3.2 GNSS观测模拟与坐标转换GNSS在真实环境中输出经纬高而EKF状态里存的是NED系位置。仿真里为了贴近实际情况会把真值NED位置转成经纬度再做一次正变换回来。这个来回转换能顺便检验坐标函数的正确性避免算法移植到实车数据时才发现坐标符号反了。function [x_ned, y_ned, z_ned] geodetic2ned(lat, lon, h, lat0, lon0, h0) % 简化坐标转换适合小范围仿真单位米 R_N 6378137; % WGS84长半轴 dLat (lat - lat0) * pi/180; dLon (lon - lon0) * pi/180; x_ned dLon * R_N * cos(lat0*pi/180); y_ned dLat * R_N; z_ned -(h - h0); end反转换写成ned2geodetic在MATLAB里直接按上式逆运算。生成GNSS观测时每隔1秒在真值位置上加高斯噪声gnss_rate 1; % GNSS更新频率1Hz sigma_gps [0.8; 0.8; 1.5]; % 水平0.8m垂直1.5m gps_idx 1; for k 1:gnss_rate*dt:... % 实际在主循环中判断 z_gps pos_ned(:,k) sigma_gps .* randn(3,1); endR矩阵由sigma_gps平方构成。注意GNSS垂直精度通常比水平差R矩阵里对应位置应给更大的方差否则EKF会把垂直方向的位置修得太激进反而放大速度误差。3.3 滤波主循环机械编排与协方差传播EKF主循环分三步标称状态机械编排预测、误差协方差传播、GNSS观测更新。标称状态里用四元数存姿态避免欧拉角奇异。% 初始化状态与协方差 state.quat [1;0;0;0]; state.vel zeros(3,1); state.pos pos_ned(:,1); state.gyro_bias zeros(3,1); state.accel_bias zeros(3,1); P blkdiag(eye(3)*0.01, eye(3)*0.1, eye(3)*0.1, eye(3)*1e-4, eye(3)*1e-4); for k 2:N dT t(k) - t(k-1); % 预测 [state, F, G, Qc] predictNominal(state, gyro(:,k), accel(:,k), dT); [F_d, Q_d] discretizeEKF(F, G, Qc, dT, euler); P F_d * P * F_d Q_d; P 0.5*(P P); % 强制对称防止数值误差破坏对称性 % GNSS更新 if mod(k, round(1/(gnss_rate*dt))) 0 z_gps pos_ned(:,k) sigma_gps .* randn(3,1); [state, P] updateEKF(state, P, z_gps, diag(sigma_gps.^2)); end % 记录位置误差、速度误差、零偏估计用于后处理 endpredictNominal函数里做四元数姿态更新和速度位置积分同时把F矩阵算出来。注意速度更新时NED系的重力向量是[0;0;9.8]加速度计输出减去零偏后加上重力再乘dtfunction [state, F, G, Qc] predictNominal(state, gyro, accel, dt) % 四元数小角度更新 dtheta gyro * dt; dq [1; 0.5*dtheta]; % 一阶近似忽略二阶小量 state.quat quatMultiply(state.quat, dq); state.quat state.quat / norm(state.quat); % 速度与位置更新 f accel - state.accel_bias; state.vel state.vel (f [0;0;9.8]) * dt; state.pos state.pos state.vel * dt; % 误差状态矩阵与上一章相同 F zeros(15,15); F(1:3,1:3) -skew(gyro); F(1:3,10:12) -eye(3); F(4:6,1:3) -skew(accel); F(4:6,13:15) -eye(3); F(7:9,4:6) eye(3); G zeros(15,12); G(1:3,1:3) -eye(3); G(4:6,4:6) -eye(3); G(10:12,7:9) eye(3); G(13:15,10:12) eye(3); Qc blkdiag(sigma_g^2*eye(3), sigma_a^2*eye(3), 1e-8*eye(3), 1e-6*eye(3)); endquatMultiply是四元数乘法MATLAB的航空航天工具箱里有现成函数没有工具箱就手写四元数乘法公式。skew用上一章提到的反对称矩阵构造。F矩阵中的gyro和accel直接取当前测量值因为误差状态线性化点的变化对结果影响很小工程上普遍这么处理。3.4 更新步观测矩阵H与卡尔曼增益GNSS位置观测直接对应误差状态中的位置误差所以观测矩阵是[0 0 I 0 0]。新息是GNSS位置减去标称状态位置修正量通过卡尔曼增益映射到15维误差状态function [state, P] updateEKF(state, P, z, R) H zeros(3,15); H(:,7:9) eye(3); % 位置观测矩阵 r z - state.pos; % 新息 S H * P * H R; K P * H / S; % 卡尔曼增益 dx K * r; % 误差状态估计 % 修正标称状态 state.pos state.pos dx(7:9); state.vel state.vel dx(4:6); state.gyro_bias state.gyro_bias dx(10:12); state.accel_bias state.accel_bias dx(13:15); % 姿态误差修正回四元数 dq [1; 0.5*dx(1:3)]; state.quat quatMultiply(state.quat, dq); state.quat state.quat / norm(state.quat); % 协方差更新标准形式 P (eye(15) - K * H) * P; P 0.5*(P P); end这里有几个参数容易看迷糊。K的维度是15x3r是3x1所以dx是15x1每个分量的顺序必须和状态定义一致。更新完成后误差状态理论上是零均值不需要把dx存进状态再清空直接修正标称状态即可。如果GNSS的数据率提高到10HzR里的方差应该保持真实GNSS精度不变而不是因为更新频率变高而缩小。4. 调参、跑通与结果验证Q/R初值、发散抑制与误差曲线判读4.1 先让仿真不崩Q和R的单位与初值EKF调参的第一步不是调增益而是把每个噪声参数的物理单位和量级写对。下面这张表是仿真初始值的常见参考范围单位很重要参数符号初值建议单位陀螺白噪声sigma_g0.01~0.1deg/s换算rad/s加速度计白噪声sigma_a0.01~0.1m/s^2陀螺零偏随机游走sigma_bg1e-4~1e-3deg/s^2加计零偏随机游走sigma_ba1e-5~1e-4m/s^3GNSS水平噪声sigma_gps_h0.5~2mGNSS垂直噪声sigma_gps_v1~3mQ矩阵里的随机游走项如果给太大零偏估计会跟着噪声乱跳位置误差反而变大给太小常值零偏要几百秒才能收敛。一个快速判断方法跑完后把零偏估计画出来如果曲线像白噪声一样剧烈抖动说明sigma_bg和sigma_ba大了一个数量级如果曲线缓慢偏离真值且长时间不回来说明Q偏小或R偏大。sigma_g 0.01*pi/180; % rad/s sigma_a 0.02; % m/s^2 sigma_bg 0.001*pi/180; % rad/s^2 sigma_ba 0.0001; % m/s^3 R_gps diag(sigma_gps.^2);这里sigma_bg的随机游走是rad/s^2不是rad/s很多仿真把单位搞错后零偏估计完全失真。零偏随机游走的物理含义是零偏变化率的功率谱密度换算时要用sigma_bg^2填进Qc对应块。4.2 R矩阵与GNSS更新率松耦合的节奏感松耦合下GNSS更新率通常只有1HzIMU是100Hz两次GNSS观测之间EKF要预测100次。R矩阵设得太小每1秒一次的修正会把位置拉得很猛速度曲线出现锯齿R设得太大GNSS信息被忽略轨迹退回纯惯导漂移。实际调参时先用GNSS标称精度设R再通过新息序列检验是否匹配。新息序列r的理论协方差是S H*P*H R因此构建卡方检验量% 在updateEKF里同时返回新息r和S chi2 r * (H*P*H R)^-1 * r; % 3自由度卡方阈值 threshold chi2inv(0.95, 3); if chi2 threshold % 新息超限可以考虑增大R或检查地图匹配异常 end这里chi2inv来自MATLAB统计工具箱。如果连续多个历元超限多半不是R参数问题而是状态已经发散或者GNSS观测出现了粗差在真实系统里应切换鲁棒更新策略而不是盲目调参。4.3 Q和R之外的发散抑制协方差限幅与零偏可观测性EKF发散最常见原因是协方差P不断缩小增益K趋近于零后续观测不再有效修正状态。这种情况在MATLAB仿真里表现为误差曲线前期收敛某时刻突然直线漂走且不再回来。一个简单有效的抑制手段是在每个周期对P的对角线设置上下限P max(P, Pmin); % 防止协方差过小 P min(P, Pmax); % 防止协方差爆炸Pmin和Pmax需要按状态变量分别给比如姿态误差给[1e-6, 1e-2]位置误差给[1e-3, 1e4]。这不是理论最优但工程上非常实用尤其可以防止长时间直线运动后水平位置协方差被压缩到无法恢复。还有一个必须注意的点零偏可观测性依赖运动激励。直线匀速运动时陀螺零偏和加速度计零偏几乎不可观测EKF只能靠过程噪声维持状态绕圈或加减速时才能把零偏估计出来。所以调零偏收敛速度时应设计包含匀速段和转弯段的轨迹否则无论Q怎么调都收敛不了。4.4 误差曲线怎么判读跑完仿真后画三张图位置误差、速度误差、陀螺与加计零偏估计。位置误差应在一个GNSS噪声标准差范围内波动且不能有单向增长趋势速度误差曲线应平滑不应出现每次GNSS更新时的尖刺零偏估计曲线应从初值缓慢收敛到预设真值附近。如果位置误差包络持续增大且呈现抛物线形态说明IMU数据里的常值零偏没有被估计出来检查EKF更新步的H矩阵是否放错了位置。如果速度误差出现周期性锯齿说明R矩阵与实际观测噪声不匹配而不是滤波算法写错。提示仿真初期前100个历元的误差尖峰不必在意那是初始协方差偏大导致的收敛过程。重点看100秒末段的稳态误差和零偏是否稳定。5. 更接近实车的玩法紧耦合、零偏在线估计与数据后处理5.1 把松耦合升级成紧耦合松耦合用GNSS的位置结果做观测信息量已经被前端RTK或定位引擎消化过一遍。紧耦合直接用伪距观测状态里要加一个接收机钟差项观测方程变成卫星伪距 几何距离 钟差 - 误差状态对位置的投影。MATLAB仿真里可以先从真值位置和卫星星历反推各通道伪距再在EKF里对每颗卫星单独建立H矩阵这样GNSS卫星少于4颗时系统仍能工作这是松耦合做不到的。紧耦合代码量比松耦合大不少但H矩阵的构造思路仍然是误差状态对观测的偏导核心框架不需要重写。5.2 零偏在线估计的精细操作零偏估计这块工程上有个通用技巧先跑一遍包含强转弯的轨迹看零偏估计是否收敛到同一组值然后把零偏从状态向量里去掉对比水平位置误差。如果去掉零偏后误差没有明显变大说明你的IMU质量好到不需要在线估计零偏直接在预测步里扣除常值补偿即可。反过来如果位置误差变化很大则必须保留零偏项。MATLAB仿真时可以用这种方式量化零偏在线估计的价值给方案设计提供数据支撑。5.3 把CSV数据接进MATLAB仿真的两个关键点用真实采集的IMU/GNSS数据做仿真时CSV导入是绕不开的第一步。要用detectImportOptions和readtimetable避免重复手写格式解析同时注意时间戳对齐opts detectImportOptions(imu.csv); tbl readtimetable(imu.csv, opts); imu_t seconds(tbl.t - tbl.t(1)); imu_data table2array(tbl(:, {gx,gy,gz,ax,ay,az})); % GNSS数据按时间戳插值到IMU时间轴 gps_t seconds(gps_tbl.t - gps_tbl.t(1)); gps_pos_interp interp1(gps_t, gps_pos, imu_t, linear, extrap);第一个关键点是时间戳必须归一化到秒且从0开始否则interp1会因为起始时间偏移产生错位第二个关键点是IMU与GNSS时间基准不一致时先做时间对齐再做坐标转换不要在坐标转换后再插值因为经纬度插值会引入非线性误差。这段对齐代码可以直接接到EKF主循环前面替换掉仿真生成的GNSS观测。本文还有配套的精品资源点击获取