如何计算GPS与IMU融合卡尔曼滤波的Q、R协方差矩阵？

针对你遇到的Q、R矩阵取值问题，结合手机传感器场景，直接给你可落地的解决思路：

一、先解决R矩阵的核心问题：坐标转换单位错误 #

你提到设小R值后结果完全跟随GPS，且当前R值异常大，大概率是GPS转NED时的单位没处理对：

• 若你直接用经纬度的原始度数计算NED坐标，1度经度/纬度对应约111km，GPS的米级误差会被放大到十万/百万级，自然需要设极大的R才能平衡IMU。
• 正确做法：将GPS经纬度通过WGS84椭球模型转换为米级的NED局部坐标（北、东方向以米为单位）。转换后，GPS的水平测量误差通常在2-5米，对应的方差就是4-25，R矩阵的对角线元素应基于这个量级设置。

R矩阵的实测方法 #

1. 把手机放在固定开阔位置（无遮挡），采集10-30分钟的GPS数据；
2. 将所有GPS数据转换为NED坐标，分别计算x（北）、y（东）方向的位置方差；
3. 直接用这个方差作为R矩阵的对角线初始值（比如x方向方差是9，就设R[0][0]=9）。

二、Q矩阵的确定：基于IMU与姿态误差的建模 #

Q是过程噪声协方差，对应系统模型的不确定性（IMU加速度误差、姿态转换误差、积分漂移等），不能凭经验设固定小值：

1. 基础：IMU静态噪声测试 #

• 将手机完全静止放置，采集5-10分钟的IMU加速度数据（已通过AHRS转换为NED坐标系）；
• 计算x、y方向加速度的方差，记为a_noise_var_x、a_noise_var_y。

2. 结合卡尔曼过程模型计算Q #

假设你的卡尔曼状态是[x, y]（仅位置），过程模型为位置由加速度积分两次得到：

x_k = x_{k-1} + v_{k-1}*dt + 0.5*a_{k-1}*dt²
y_k = y_{k-1} + v_{k-1}*dt + 0.5*a_{k-1}*dt²

过程噪声主要来自加速度的误差，Q的对角线元素可通过以下公式估算：

Q_x = (0.5 * dt²)² * a_noise_var_x
Q_y = (0.5 * dt²)² * a_noise_var_y

其中dt是IMU的采样间隔（比如10Hz采样，dt=0.1s）。

3. 加入姿态误差的修正 #

AHRS的横滚、俯仰、偏航误差会导致加速度转换到NED时出现偏差，这部分也需加入Q：

• 若AHRS的姿态角误差方差为att_err_var（比如偏航角误差1度，转弧度后方差≈(π/180)²≈0.0003），可计算姿态误差带来的加速度转换误差方差，加到a_noise_var中再计算Q。

三、调试技巧 #

1. 先固定R为实测的GPS方差，调整Q：
   • 若融合结果漂移过快（IMU积分误差累积太快），说明Q设小了，增大Q；
   • 若融合结果过于跟随IMU（GPS修正不足），说明Q设大了，减小Q。
2. 再固定Q，微调R：
   • 若GPS跳变直接体现在融合结果中，说明R设小了，增大R；
   • 若融合结果跟不上真实移动（GPS权重太低），说明R设大了，减小R。

内容的提问来源于stack exchange，提问作者babyCoder