给 Cssrlib加注释（6）--卡尔曼滤波

数据处理与理论补充3--卡尔曼滤波

在矩阵构建好后进行迭代计算，这里采用的是卡尔曼滤波，卡尔曼滤波的核心思想是平衡一步预测与观测值，权重的计算由二者的方差决定，如果要采用卡尔曼滤波，首先要明确系统的状态方程和观测方程[5]：

式中，Xk表示状态向量，Φk表示状态转移矩阵，Wk表示系统噪声，Hk表示观测方程的系数阵（设计矩阵），Vk为观测噪声

其中，噪声满足下式：

其中，Qk和Rk表示的是系统噪声的方差阵和两侧噪声的方差阵。

其中，噪声我们在前文已经设置好了，H阵在上一板块也构建好了，还差状态转移矩阵，在PPP中，认为状态转移矩阵为单位阵，即把上一时刻历元的位置当作这一时刻的预测，对于静态定位肯定是说的同的，对于动态定位，可以通过设置一个很大的方差来表示更信任观测。 接下来我们可以进行滤波更新，对着理论把代码附上：

在代码中，首先更计算的是滤波增益：

        PHt = P@H.T
        S = H@PHt+R
        K = PHt@np.linalg.inv(S)

对应公式：

这个K我的理解就是权重，权重是由噪声的方差阵来决定的（R表示测量噪声）

然后就可以估计状态（加权）：

        x += K@v

v表示上一板块计算的残差，对应公式为：

然后计算估计得到的X的方差阵

        IKH = np.eye(P.shape[0])-K@H
        P = IKH@P@IKH.T + K@R@K.T  # Joseph stabilized version

对应公式为：

式中，I为单位阵

这样就完成了卡尔曼滤波，接下来代码里计算了后验残差，更新了矩阵为下一时刻准备：

       yu, eu, elu = self.zdres(obs, cs, bsx, rs, vs, dts, xp[0:3])
        y = yu[iu, :]
        e = eu[iu, :]
        ny = y.shape[0]
        if ny < 6:
            return -1
        # Residuals for float solution
        #
        v, H, R = self.sdres(obs, xp, y, e, sat, el)

至此，整个PPP结束，由于HAS暂时不播发相位改正数，因此推测没办法进行模糊度固定，之前答应大家的可能要后面再更新~

结语

这是本人第一次完整的看完一遍PPP过程，看完之后才知道自己所知甚少还需日后精进，奈何才疏学浅肯定漏洞百出还望大家指出，后面可能会更新轨道钟差融合部分的理论和代码，就算是为了自己的不甘心吧

我们的故事告一段落

参考文献

[1] 宋丹丹. 北斗导航定位解算算法的研究与软件实现[D].西安电子科技大学,2018.

[2] Galileo_HAS_SIS_ICD_v1.0

[3] 李昕. 多频率多星座GNSS快速精密定位关键技术研究[D].武汉大学,2022.

[4] 《GNSS精密单点定位理论方法及其应用》

[5] 李星星.GNSS精密单点定位及非差模糊度快速确定方法研究[D].武汉大学,2013.

[6] 周锋.多系统GNSS非差非组合精密单点定位相关理论和方法研究[D].华东师范大学,2018