如需帮助，或有导航、定位滤波相关的代码定制需求，可点击文末卡片联系作者
文章目录
无人机惯性地磁测速定位系统（Inertial/Magnetic Navigation System）是 GPS 拒止环境下的核心替代方案，将 INS （惯性导航）的短期精度与地磁匹配的绝对位置修正能力结合，实现长时间自主导航。
核心模块详解
惯性导航解算
INS 的核心是对加速度计和陀螺仪数据做积分，误差随时间二次累积（位置误差 ∝ t²）。状态方程在导航系（NED）下：
p ˙ = v , v ˙ = C b n f b − ( 2 ω i e n + ω e n n ) × v + g n \dot{\mathbf{p}} = \mathbf{v}, \quad \dot{\mathbf{v}} = \mathbf{C}b^n \mathbf{f}^b - (2\boldsymbol{\omega}{ie}^n + \boldsymbol{\omega}_{en}^n) \times \mathbf{v} + \mathbf{g}^n p˙=v,v˙=Cbnfb−(2ωien+ωenn)×v+gn
其中 C b n \mathbf{C}_b^n Cbn 是体坐标系到导航系的方向余弦矩阵，由四元数传播更新以避免欧拉角奇异性。
磁场测量与补偿
原始磁力计输出受两类误差污染，必须在进 EKF 之前校准：
硬磁误差 （Hard iron）：机体上永磁材料产生固定偏置 b \mathbf{b} b，在测量球面上表现为中心偏移。
软磁误差 （Soft iron）：导磁材料改变磁场方向，使球面畸变为椭球面，用矩阵 A \mathbf{A} A 描述。
校准模型： m c a l = A − 1 ( m r a w − b ) \mathbf{m}{cal} = \mathbf{A}^{-1}(\mathbf{m}{raw} - \mathbf{b}) mcal=A−1(mraw−b)
在飞行前做"8字形"机动，用最小二乘椭球拟合求解 A \mathbf{A} A 和 b \mathbf{b} b。
EKF 状态向量设计
EKF 是整个系统的核心，19维状态向量：
x = [ p n ⏟ 3 , v n ⏟ 3 , δ θ ⏟ 3 , b g ⏟ 3 , b a ⏟ 3 , b m ⏟ 3 ] T \mathbf{x} = [\underbrace{\mathbf{p}^n}{3}, \underbrace{\mathbf{v}^n}{3}, \underbrace{\boldsymbol{\delta\theta}}_{3}, \underbrace{\mathbf{b}g}{3}, \underbrace{\mathbf{b}a}{3}, \underbrace{\mathbf{b}m}{3}]^T x=[3 pn,3 vn,3 δθ,3 bg,3 ba,3 bm]T
观测更新：将校准后的磁力计输出与地磁模型预测值（来自 WMM/IGRF）做差，形成观测残差，驱动状态更新。
z m = m c a l − C b n T m r e f ( p ) \mathbf{z}m = \mathbf{m}{cal} - \mathbf{C}b^n{}^T \mathbf{m}{ref}(\mathbf{p}) zm=mcal−CbnTmref(p)
地磁匹配定位（Geomagnetic Map Matching）
这是获得绝对位置的关键。常用算法：
ICCP（迭代最近等值线点）：将实时测量的磁场序列与预存地图做轮廓匹配，类似地形辅助导航（TERCOM）的思路，适合磁场特征丰富的区域。
MSD（均方差）匹配：在候选位置窗口内，计算实测序列与地图的均方差，取最小值处为匹配位置，计算简单但依赖地图分辨率。
粒子滤波：用数千个粒子表示位置概率分布，每步根据磁场似然更新粒子权重，无需线性化假设，对非高斯误差鲁棒性强。
速度辅助：ZUPT 与磁场测速
ZUPT（零速修正）：当无人机短暂悬停或检测到静止时，将速度强制约束为零，大幅抑制 IMU 漂移。
地磁测速（Magnetic Odometry）：利用无人机穿越空间磁场梯度时，磁力计读数的变化率估计运动速度：
v ^ ≈ − ( ∇ m m a p ) + m ˙ m e a s \hat{\mathbf{v}} \approx -\left(\nabla \mathbf{m}{map}\right)^+ \dot{\mathbf{m}}{meas} v^≈−(∇mmap)+m˙meas
即用地图梯度的伪逆乘以实测磁场时间导数，需要高精度地图且只在梯度显著区域有效。
MATLAB 实现框架
以下是 EKF 核心循环的 MATLAB 代码框架：
matlab
%% 无人机惯性地磁融合导航 - EKF 主循环
% 状态维度：位置(3) + 速度(3) + 姿态误差角(3) + 陀螺漂移(3) + 加速度偏差(3) + 磁偏差(3) = 19
function nav = runMagINS_EKF(imu_data, mag_data, wmm_map, dt)
n = 19; % 状态维度
% ---- 初始化 ----
x = zeros(n, 1); % 误差状态（间接法 EKF）
P = diag([...
[1 1 1]*5^2, ... % 位置初始方差 (m^2)
[1 1 1]*0.1^2, ... % 速度初始方差 (m/s)^2
[1 1 1]*(1*pi/180)^2,... % 姿态角误差 (rad^2)
[1 1 1]*(0.01*pi/180)^2,... % 陀螺漂移
[1 1 1]*(0.01)^2, ... % 加速度偏差
[1 1 1]*(100e-9)^2]); % 磁偏差 (T^2)，约 100 nT
% 过程噪声（连续时间转离散时间）
q_gyro = (0.003 * pi/180)^2; % rad^2/s（陀螺 ARW）
q_accel = (0.05 * 9.8e-3)^2; % (m/s^2)^2（加速度计噪声）
q_mag = (10e-9)^2; % T^2（磁力计量测噪声）
Q = buildProcessNoise(q_gyro, q_accel, dt, n);
% 量测噪声
R_mag = eye(3) * q_mag;
% ---- 惯性导航真实状态（参考量）----
nav.pos = zeros(3, size(imu_data,2));
nav.vel = zeros(3, size(imu_data,2));
nav.quat = [1;0;0;0]; % 初始四元数 w,x,y,z
for k = 1 : size(imu_data, 2)
% ==== 步骤1：INS 力学方程推进（预测） ====
gyro = imu_data(1:3, k); % 角速度 rad/s（已减去地球自转，简化）
accel = imu_data(4:6, k); % 比力 m/s^2（体系）
% 四元数更新（龙格-库塔或一阶近似）
nav.quat = quatUpdate(nav.quat, gyro, dt);
Cbn = quat2dcm(nav.quat); % 体系 -> 导航系
% 速度和位置积分（简化：忽略科里奥利，适用于低速短时）
a_nav = Cbn * accel + [0;0;9.8]; % 减去重力
nav.vel(:,k+1) = nav.vel(:,k) + a_nav * dt;
nav.pos(:,k+1) = nav.pos(:,k) + nav.vel(:,k) * dt;
% ==== 步骤2：EKF 预测（协方差传播） ====
F = buildStateTransMatrix(Cbn, accel, dt, n); % 系统矩阵线性化
P = F * P * F' + Q;
% ==== 步骤3：磁力计更新（量测） ====
if mod(k, 5) == 0 % 磁力计 50Hz，IMU 250Hz，每 5 步更新一次
mag_meas = mag_data(:, floor(k/5)); % 已校准磁力计读数（T）
% 从地磁模型查参考磁场（导航系下）
[mN, mE, mD] = queryWMM(wmm_map, nav.pos(:,k+1));
m_ref = Cbn' * [mN; mE; mD]; % 转到体系与测量比较
% 量测残差
z = mag_meas - m_ref;
% 量测矩阵 H（对磁偏差状态的偏导数）
H = buildMagMeasMatrix(Cbn, [mN;mE;mD], n);
% 卡尔曼增益与更新
S = H * P * H' + R_mag;
K = P * H' / S;
x = x + K * z;
P = (eye(n) - K * H) * P * (eye(n) - K * H)' + K * R_mag * K';
% 用误差状态修正真实状态（间接法反馈）
nav.pos(:,k+1) = nav.pos(:,k+1) + x(1:3);
nav.vel(:,k+1) = nav.vel(:,k+1) + x(4:6);
nav.quat = quatCorrect(nav.quat, x(7:9));
x = zeros(n,1); % 重置误差状态
end
end
end关键工程难点
地磁地图质量是精度瓶颈。WMM/IGRF 全球模型分辨率约 50km，只够初始粗定位；实际工程需要提前飞航线建立高精度局部地磁地图，分辨率达到 1--10m 量级。
磁场异常干扰 （城市钢筋建筑、地铁、高压线）会使匹配完全失效，需要在线异常检测，自动降低磁力计量测的权重（增大 R m a g R_{mag} Rmag）。
可观测性分析：纯磁场对 Z 轴（航向）的可观测性在低速、低机动时很差，需要结合加速度计机动来激励航向可观测性，这是 EKF 收敛的必要条件。
UKF vs EKF：磁场模型的非线性较强，当姿态误差较大时 EKF 线性化误差显著，可改用 UKF（无迹变换），以 sigma 点传播替代雅可比矩阵，代价是计算量约增加 2--3 倍。