基于GPS+IMU的卡尔曼滤波融合定位算法matlab代码 其中惯导用来进行状态预测,GPS用来滤波矫正，用于GPS+IMU的卡尔曼滤波融合定位算法算法编程学习！！！

最近在搞无人车定位，发现GPS和IMU单独用都不靠谱。GPS信号飘得亲妈都不认识，IMU积分漂移能把你带到姥姥家。这俩货刚好互补，卡尔曼滤波一搅和——哎，定位突然就老实了。今天咱们手撕代码，看看这玩意儿到底怎么玩的。

先整点核心代码热热身：

% 状态向量 [x; vx; y; vy; bias_ax; bias_ay]
x = zeros(6,1);  
P = eye(6)*10;   % 初始协方差矩阵
Q = diag([0.1, 0.1, 0.1, 0.1, 0.01, 0.01]);  % 过程噪声
R = diag([3,3]);  % 观测噪声

这六个状态变量藏了不少心眼。x、y位置和速度好理解，biasax和biasay是加速度计零偏。实际开发时IMU的零偏会随时间漂移，必须当状态变量估计。见过有人偷懒不加零偏估计，结果半小时后定位漂出二里地，场面十分下饭。

预测环节是IMU的主场：

% 状态转移矩阵
dt = 0.1;
F = [1 dt 0  0  0  0;
     0  1 0  0 -dt 0;
     0  0 1 dt 0  0;
     0  0 0  1 0 -dt;
     0  0 0  0  1  0;
     0  0 0  0  0  1];
     
% 控制输入矩阵（加速度计读数）
accel = [ax; ay]; 
B = [0.5*dt^2 0;
     dt       0;
     0        0.5*dt^2;
     0        dt;
     0        0;
     0        0];
     
x = F*x + B*accel;
P = F*P*F' + Q;

这个F矩阵设计有讲究。注意bias项如何影响速度预测——IMU测量的加速度减去零偏才是真实加速度。B矩阵把加速度转换成速度变化和位置变化，0.5at²这公式是不是让你想起高中物理？

重点来了，GPS更新阶段：

H = [1 0 0 0 0 0;
     0 0 1 0 0 0];  % 仅观测位置

K = P*H'/(H*P*H' + R);
x = x + K*(gps_measurement - H*x);
P = (eye(6) - K*H)*P;

H矩阵像个挑食的小孩，只认位置信息。实际项目里GPS也可能给速度观测，这时候H矩阵得改。K矩阵就是传说中的卡尔曼增益，决定了相信预测多一点还是观测多一点。有次我把R调太小，结果GPS一跳变整个轨迹跟着抽风，直接上演蛇形走位。

完整流程在循环里转起来：

while ~stop
    % IMU数据到达
    [accel, gyro] = read_IMU();
    predict_step(accel);
    
    % GPS数据到达
    if gps_available()
        update_step(gps_pos);
    end
    
    trajectory = [trajectory; x(1), x(3)];
end

这里有个坑——IMU和GPS数据频率不同。IMU通常100Hz往上，GPS也就10Hz。处理时要么用异步卡尔曼，要么做数据对齐。新手容易在这儿翻车，出现时间不同步还怪算法不行。

调参是门玄学。Q矩阵对角线元素我一般从0.01开始试，R矩阵参考GPS厂商给的精度。有个邪门操作：在地下停车场把Q调大让算法更相信GPS，结果...好吧车还是迷路了，但至少知道它迷路的位置不是？

跑起来的轨迹刚开始会扭得像麻花，等零偏估计收敛后就老实了。见过最骚的操作是拿扩展卡尔曼处理IMU的非线性误差，不过咱们这个简易版已经能应付低速场景。下次试试融合轮速计，那又是另一个故事了。