GPS伪距EKF实现时H矩阵正确但残差异常偏大问题咨询

核心问题说明 #

你的观测残差计算逻辑错误，确实不能用H*stateVec计算预测观测值，必须直接调用非线性观测方程计算预测伪距。

错误原因分析 #

EKF的线性化本质是对观测残差相对于状态量变化的一阶近似，而非用线性化的H矩阵直接替代原始非线性观测方程。你现在得到的1e7量级残差，本质是H*stateVec的物理意义本身就不是伪距，和真实观测伪距自然存在量级级的偏差。
你之前在最小二乘中使用H矩阵正常，是因为加权最小二乘迭代的逻辑本身就是：

• 先基于当前状态初值用非线性观测方程算预测伪距，得到残差z - h(x0)
• 再用H矩阵计算状态增量dx = (H^T R^{-1} H)^{-1} H^T R^{-1} (z - h(x0))
  你在最小二乘中也不会用H*x0作为伪距预测值，只是把EKF的更新步骤搞混了。

正确更新步骤 #

1. 基于当前的状态预测值stateVec = [x, y, z, cdt]，对每颗卫星计算非线性观测预测值：
   h_i = sqrt( (x_sat_i - x)^2 + (y_sat_i - y)^2 + (z_sat_i - z)^2 ) + cdt
   将所有卫星的h_i拼接得到预测观测向量h_pred
2. 观测残差计算为res = Z - h_pred，这个残差的正常量级应该在米级，不会出现1e7的异常值
3. 你现有的H矩阵计算逻辑是正确的，可直接沿用
4. 状态更新为stateVec = stateVec + K * res，后续协方差更新按EKF标准流程执行即可

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