惯导里程计GPS组合导航算法，matlab代码卡尔曼滤波

当咱们的扫地机器人在客厅迷路时，惯性导航（INS）和GPS的组合理念就派上用场了。惯性器件天生自带误差累积的debuff，这时候就需要GPS这个外挂来定期修正。今天咱们用Matlab撸个简易版组合导航算法，感受下卡尔曼滤波如何玩转传感器融合。

先甩个系统模型定义：

% 状态向量 [x位置误差, x速度误差, 陀螺零偏, y位置误差, y速度误差, 加速度计零偏]
F = [1 dt 0   0 0 0; 
     0 1 -dt 0 0 0;
     0 0 1   0 0 0;
     0 0 0   1 dt 0;
     0 0 0   0 1 dt;
     0 0 0   0 0 1]; % 状态转移矩阵

H = [1 0 0 0 0 0; 
     0 0 0 1 0 0]; % 观测矩阵

Q = diag([0.1, 0.2, 0.01, 0.1, 0.2, 0.01]); % 过程噪声
R = eye(2)*5; % 观测噪声

这里有个小技巧：状态矩阵F里的dt项其实暗藏玄机。当采样时间间隔变化时，这些微分项的系数会自动跟着调整，相当于给系统装了个自适应齿轮。

接着是卡尔曼的经典五连击：

function [x_est, P_est] = kalman_update(x_pred, P_pred, z)
    % 预测与现实的碰撞
    K = P_pred * H' / (H * P_pred * H' + R); % 最优增益计算
    x_est = x_pred + K*(z - H*x_pred);       % 状态修正
    P_est = (eye(6) - K*H)*P_pred;           % 协方差更新
    
    % 防止协方差矩阵发疯
    P_est = (P_est + P_est')/2;              % 强制对称
    [V,D] = eig(P_est);
    D(D<0) = 0;                              % 特征值截断
    P_est = V*D/V;
end

注意那个特征值截断操作，这相当于给滤波器上了个保险。当数值计算导致协方差矩阵不正定时，这个操作能避免整个系统崩掉——实测中救过不少次场。

惯导里程计GPS组合导航算法，matlab代码卡尔曼滤波

数据融合环节才是重头戏：

% 模拟数据生成
true_pos = cumsum(randn(100,2)*0.1);         % 真实轨迹
ins_error = cumsum(randn(100,2)*0.5);        % 惯导误差累积
gps_noise = randn(size(true_pos))*1.5;       % GPS噪声

% 融合处理
fused_pos = zeros(size(true_pos));
x_est = zeros(6,1);
P_est = eye(6)*10;
for k = 1:length(true_pos)
    % 预测步
    x_pred = F * x_est;
    P_pred = F * P_est * F' + Q;
    
    % 观测到来时更新
    if mod(k,5)==0  % 模拟GPS 5Hz更新
        z = (true_pos(k,:) + gps_noise(k,:))' - ins_pos(k,:)';
        [x_est, P_est] = kalman_update(x_pred, P_pred, z);
    else
        x_est = x_pred;
        P_est = P_pred;
    end
    
    % 误差补偿
    fused_pos(k,:) = ins_pos(k,:) + [x_est(1), x_est(4)]';
end

这个5Hz的更新频率设定是个经验值。实际项目中要根据传感器特性调整——比如无人机高速运动时需要更高频的GPS修正，而扫地机器人这种慢速移动的可以适当放宽。

跑完仿真后对比轨迹，会发现融合后的路径既保留了INS的高频响应特性，又抑制了长期漂移。这就像给惯导加了根GPS做的缰绳，既不让它野马脱缰，又不限制其灵活身手。

最后给个避坑指南：实际部署时要特别注意时间同步问题。别让IMU和GPS的时间戳差个几毫秒，否则滤波效果会像没对齐的齿轮一样互相较劲。曾经有个项目因为这个细节没处理好，定位误差硬是比预期大了三倍，血泪教训啊！