惯导里程计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(D0) 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的时间戳差个几毫秒否则滤波效果会像没对齐的齿轮一样互相较劲。曾经有个项目因为这个细节没处理好定位误差硬是比预期大了三倍血泪教训啊