让GNSSRTK不再难【第二天-第7部分2】

状态更新计算过程：

1. 计算卡尔曼增益：
   根据预测的误差协方差矩阵 $P_k^{-}$ 和观测噪声协方差矩阵 $R$ 计算卡尔曼增益 $K_k$：
   $$K_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}$$

   带入预测的 $P_k^{-}$ 和 $R$ 计算：

   $$P_k^{-}=\left[\begin{array}{cccccc}Co{v}_{XX}& 0& 0& 0& 0& 0\\ 0& Co{v}_{YY}& 0& 0& 0& 0\\ 0& 0& Co{v}_{ZZ}& 0& 0& 0\\ 0& 0& 0& Co{v}_{\delta t\delta t}& 0& 0\\ 0& 0& 0& 0& \ast & \ast \\ 0& 0& 0& 0& \ast & \ast \end{array}\right]$$

   $$R=\left[\begin{array}{cccc}{\sigma }_1^2& 0& \cdots & 0\\ 0& {\sigma }_2^2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & {\sigma }_n^2\end{array}\right]$$

   假设观测矩阵 $H$ 为设计矩阵 $A$：

   $$A=\left[\begin{array}{ccccccc}{l}_{f_1}^{G_1}& m_{f_1}^{G_1}& n_{f_1}^{G_1}& -1& 0& 0& 0\\ l_{f_2}^{G_2}& m_{f_2}^{G_2}& n_{f_2}^{G_2}& -1& 0& 0& 0\\ l_{f_3}^{G_3}& m_{f_3}^{G_3}& n_{f_3}^{G_3}& -1& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{G_n}& m_{f_n}^{G_n}& n_{f_n}^{G_n}& -1& 0& 0& 0\\ l_{f_1}^{C_1}& m_{f_1}^{C_1}& n_{f_1}^{C_1}& -1& 0& -1& 0\\ l_{f_2}^{C_2}& m_{f_2}^{C_2}& n_{f_2}^{C_2}& -1& 0& -1& 0\\ l_{f_3}^{C_3}& m_{f_3}^{C_3}& n_{f_3}^{C_3}& -1& 0& -1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{C_n}& m_{f_n}^{C_n}& n_{f_n}^{C_n}& -1& 0& -1& 0\end{array}\right]$$

   则卡尔曼增益 $K_k$ 计算为：
   $$K_k=P_k^{-}{A}^T(A{P}_k^{-}{A}^T+R{)}^{-1}$$

   （1）计算 $A{P}_k^{-}{A}^T$：
   $$A{P}_k^{-}{A}^T=A\left[\begin{array}{cccccc}Co{v}_{XX}& 0& 0& 0& 0& 0\\ 0& Co{v}_{YY}& 0& 0& 0& 0\\ 0& 0& Co{v}_{ZZ}& 0& 0& 0\\ 0& 0& 0& Co{v}_{\delta t\delta t}& 0& 0\\ 0& 0& 0& 0& \ast & \ast \\ 0& 0& 0& 0& \ast & \ast \end{array}\right]A^T$$

   （2）计算 $A{P}_k^{-}{A}^T+R$：
   $$A{P}_k^{-}{A}^T+R=A\left[\begin{array}{cccccc}Co{v}_{XX}& 0& 0& 0& 0& 0\\ 0& Co{v}_{YY}& 0& 0& 0& 0\\ 0& 0& Co{v}_{ZZ}& 0& 0& 0\\ 0& 0& 0& Co{v}_{\delta t\delta t}& 0& 0\\ 0& 0& 0& 0& \ast & \ast \\ 0& 0& 0& 0& \ast & \ast \end{array}\right]A^T+R$$

   由于 $A{P}_k^{-}{A}^T+R$ 是对角矩阵，其逆矩阵为：
   $$(A{P}_k^{-}{A}^T+R{)}^{-1}=\left[\begin{array}{ccc}(Co{v}_{XX}+{\sigma }_1^2{)}^{-1}& 0& 0\\ 0& (Co{v}_{YY}+{\sigma }_2^2{)}^{-1}& 0\\ 0& 0& (Co{v}_{ZZ}+{\sigma }_3^2{)}^{-1}\end{array}\right]$$

   （3）计算 $K_k$：
   $$K_k=P_k^{-}{A}^T(A{P}_k^{-}{A}^T+R{)}^{-1}$$

   带入 $P_k^{-}$ 和 $(A{P}_k^{-}{A}^T+R{)}^{-1}$：
   $$K_k=\left[\begin{array}{ccc}Co{v}_{XX}& 0& 0\\ 0& Co{v}_{YY}& 0\\ 0& 0& Co{v}_{ZZ}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]A^T\left[\begin{array}{ccc}(Co{v}_{XX}+{\sigma }_1^2{)}^{-1}& 0& 0\\ 0& (Co{v}_{YY}+{\sigma }_2^2{)}^{-1}& 0\\ 0& 0& (Co{v}_{ZZ}+{\sigma }_3^2{)}^{-1}\end{array}\right]$$

   简化计算得到：
   $$K_k=\left[\begin{array}{ccc}Co{v}_{XX}(Co{v}_{XX}+{\sigma }_1^2{)}^{-1}& 0& 0\\ 0& Co{v}_{YY}(Co{v}_{YY}+{\sigma }_2^2{)}^{-1}& 0\\ 0& 0& Co{v}_{ZZ}(Co{v}_{ZZ}+{\sigma }_3^2{)}^{-1}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$$

   因此，卡尔曼增益 $K_k$ 为：
   $$K_k=\left[\begin{array}{ccc}\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}& 0& 0\\ 0& \frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}& 0\\ 0& 0& \frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$$

2. 更新状态估计：
   根据观测值 $z_k$ 和预测值 ${\hat{x}}_k^{-}$ 进行状态更新：
   $$x_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$$

   带入观测值 $z_k$ 和预测值 ${\hat{x}}_k^{-}$：

   假设 ${\hat{x}}_k^{-}$ 为：
   $${\hat{x}}_k^{-}=\left[\begin{array}{c}{\hat{x}}_{k,1}^{-}\\ {\hat{x}}_{k,2}^{-}\\ {\hat{x}}_{k,3}^{-}\\ ⋮\\ {\hat{x}}_{k,7}^{-}\end{array}\right]$$

   观测值 $z_k$ 为：
   $$z_k=\left[\begin{array}{c}{z}_{k,1}\\ z_{k,2}\\ z_{k,3}\end{array}\right]$$

   假设观测矩阵 $H$ 为设计矩阵 $A$：

   $$A=\left[\begin{array}{ccccccc}{l}_{f_1}^{G_1}& m_{f_1}^{G_1}& n_{f_1}^{G_1}& -1& 0& 0& 0\\ l_{f_2}^{G_2}& m_{f_2}^{G_2}& n_{f_2}^{G_2}& -1& 0& 0& 0\\ l_{f_3}^{G_3}& m_{f_3}^{G_3}& n_{f_3}^{G_3}& -1& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{G_n}& m_{f_n}^{G_n}& n_{f_n}^{G_n}& -1& 0& 0& 0\\ l_{f_1}^{C_1}& m_{f_1}^{C_1}& n_{f_1}^{C_1}& -1& 0& -1& 0\\ l_{f_2}^{C_2}& m_{f_2}^{C_2}& n_{f_2}^{C_2}& -1& 0& -1& 0\\ l_{f_3}^{C_3}& m_{f_3}^{C_3}& n_{f_3}^{C_3}& -1& 0& -1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{C_n}& m_{f_n}^{C_n}& n_{f_n}^{C_n}& -1& 0& -1& 0\end{array}\right]$$

   则状态更新为：

   $$x_k=\left[\begin{array}{c}{\hat{x}}_{k,1}^{-}\\ {\hat{x}}_{k,2}^{-}\\ {\hat{x}}_{k,3}^{-}\\ ⋮\\ {\hat{x}}_{k,7}^{-}\end{array}\right]+K_k\left(\left[\begin{array}{c}{z}_{k,1}\\ z_{k,2}\\ z_{k,3}\end{array}\right]-A\left[\begin{array}{c}{\hat{x}}_{k,1}^{-}\\ {\hat{x}}_{k,2}^{-}\\ {\hat{x}}_{k,3}^{-}\\ ⋮\\ {\hat{x}}_{k,7}^{-}\end{array}\right]\right)$$

   简化后：
   $$x_k=\left[\begin{array}{c}{\hat{x}}_{k,1}^{-}\\ {\hat{x}}_{k,2}^{-}\\ {\hat{x}}_{k,3}^{-}\\ ⋮\\ {\hat{x}}_{k,7}^{-}\end{array}\right]+K_k\left[\begin{array}{c}{z}_{k,1}-(A{\hat{x}}_k^{-}{)}_1\\ z_{k,2}-(A{\hat{x}}_k^{-}{)}_2\\ z_{k,3}-(A{\hat{x}}_k^{-}{)}_3\end{array}\right]$$

   带入卡尔曼增益 $K_k$ 计算结果：
   $$K_k=\left[\begin{array}{ccc}\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}& 0& 0\\ 0& \frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}& 0\\ 0& 0& \frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$$

   最终状态更新为：
   $$x_k=\left[\begin{array}{c}{\hat{x}}_{k,1}^{-}\\ {\hat{x}}_{k,2}^{-}\\ {\hat{x}}_{k,3}^{-}\\ ⋮\\ {\hat{x}}_{k,7}^{-}\end{array}\right]+\left[\begin{array}{ccc}\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}& 0& 0\\ 0& \frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}& 0\\ 0& 0& \frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{z}_{k,1}-(A{\hat{x}}_k^{-}{)}_1\\ z_{k,2}-(A{\hat{x}}_k^{-}{)}_2\\ z_{k,3}-(A{\hat{x}}_k^{-}{)}_3\end{array}\right]$$

3. 更新误差协方差矩阵：
   更新误差协方差矩阵 $P_k$：

   $$P_k=(I-K_{k}A)P_k^{-}$$

   带入计算：

   假设 $I$ 为单位矩阵：
   $$I=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

   卡尔曼增益 $K_k$ 为：
   $$K_k=\left[\begin{array}{ccc}\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}& 0& 0\\ 0& \frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}& 0\\ 0& 0& \frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$$

   假设观测矩阵 $H$ 为设计矩阵 $A$：
   $$A=\left[\begin{array}{ccccccc}{l}_{f_1}^{G_1}& m_{f_1}^{G_1}& n_{f_1}^{G_1}& -1& 0& 0& 0\\ l_{f_2}^{G_2}& m_{f_2}^{G_2}& n_{f_2}^{G_2}& -1& 0& 0& 0\\ l_{f_3}^{G_3}& m_{f_3}^{G_3}& n_{f_3}^{G_3}& -1& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{G_n}& m_{f_n}^{G_n}& n_{f_n}^{G_n}& -1& 0& 0& 0\\ l_{f_1}^{C_1}& m_{f_1}^{C_1}& n_{f_1}^{C_1}& -1& 0& -1& 0\\ l_{f_2}^{C_2}& m_{f_2}^{C_2}& n_{f_2}^{C_2}& -1& 0& -1& 0\\ l_{f_3}^{C_3}& m_{f_3}^{C_3}& n_{f_3}^{C_3}& -1& 0& -1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{C_n}& m_{f_n}^{C_n}& n_{f_n}^{C_n}& -1& 0& -1& 0\end{array}\right]$$

   则更新误差协方差矩阵为：
   $$P_k=\left(\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]-\left[\begin{array}{ccc}\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}& 0& 0\\ 0& \frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}& 0\\ 0& 0& \frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{ccccccc}{l}_{f_1}^{G_1}& m_{f_1}^{G_1}& n_{f_1}^{G_1}& -1& 0& 0& 0\\ l_{f_2}^{G_2}& m_{f_2}^{G_2}& n_{f_2}^{G_2}& -1& 0& 0& 0\\ l_{f_3}^{G_3}& m_{f_3}^{G_3}& n_{f_3}^{G_3}& -1& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{G_n}& m_{f_n}^{G_n}& n_{f_n}^{G_n}& -1& 0& 0& 0\\ l_{f_1}^{C_1}& m_{f_1}^{C_1}& n_{f_1}^{C_1}& -1& 0& -1& 0\\ l_{f_2}^{C_2}& m_{f_2}^{C_2}& n_{f_2}^{C_2}& -1& 0& -1& 0\\ l_{f_3}^{C_3}& m_{f_3}^{C_3}& n_{f_3}^{C_3}& -1& 0& -1& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ l_{f_n}^{C_n}& m_{f_n}^{C_n}& n_{f_n}^{C_n}& -1& 0& -1& 0\end{array}\right]\right)\left[\begin{array}{cccccc}Co{v}_{XX}& 0& 0& 0& 0& 0\\ 0& Co{v}_{YY}& 0& 0& 0& 0\\ 0& 0& Co{v}_{ZZ}& 0& 0& 0\\ 0& 0& 0& Co{v}_{\delta t\delta t}& 0& 0\\ 0& 0& 0& 0& \ast & \ast \\ 0& 0& 0& 0& \ast & \ast \end{array}\right]$$

   进一步计算得到：进一步计算得到：
   $$P_k=\left(\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]-\left[\begin{array}{ccc}\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}& 0& 0\\ 0& \frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}& 0\\ 0& 0& \frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\right)\left[\begin{array}{cccccc}Co{v}_{XX}& 0& 0& 0& 0& 0\\ 0& Co{v}_{YY}& 0& 0& 0& 0\\ 0& 0& Co{v}_{ZZ}& 0& 0& 0\\ 0& 0& 0& Co{v}_{\delta t\delta t}& 0& 0\\ 0& 0& 0& 0& \ast & \ast \\ 0& 0& 0& 0& \ast & \ast \end{array}\right]$$

   最终得到：
   $$P_k=\left[\begin{array}{cccccc}\left(1-\frac{Co{v}_{XX}}{Co{v}_{XX}+{\sigma }_1^2}\right)Co{v}_{XX}& 0& 0& 0& 0& 0\\ 0& \left(1-\frac{Co{v}_{YY}}{Co{v}_{YY}+{\sigma }_2^2}\right)Co{v}_{YY}& 0& 0& 0& 0\\ 0& 0& \left(1-\frac{Co{v}_{ZZ}}{Co{v}_{ZZ}+{\sigma }_3^2}\right)Co{v}_{ZZ}& 0& 0& 0\\ 0& 0& 0& Co{v}_{\delta t\delta t}& 0& 0\\ 0& 0& 0& 0& \ast & \ast \\ 0& 0& 0& 0& \ast & \ast \end{array}\right]$$

16.2 站星双差Kalman滤波伪距差分定位流程

站星双差仅有位置状态量，所以其Kalman滤波流程更加简单。

对于时间更新步骤，基本就使用单点结果对概略位置进行填充，所以实际上已经降级为最小二乘，因为前后历元状态量在时间序列上不存在相关性。

但对于观测更新过程，观测值的方差需要考虑因星间作差引入的相关性。

对于没有做星间单差之前

$$V_{ud}=\left[\begin{array}{c}{p}^1\\ p^2\\ p^3\\ p^4\end{array}\right]R_{ud}=\left[\begin{array}{cccc}{\sigma }_1^2& 0& 0& 0\\ 0& {\sigma }_2^2& 0& 0\\ 0& 0& {\sigma }_3^2& 0\\ 0& 0& 0& {\sigma }_4^2\end{array}\right]$$

星间单差之后

$$V_{sd}=\left[\begin{array}{c}{p}^2-p^1\\ p^3-p^1\\ p^4-p^1\end{array}\right]R_{sd}=\left[\begin{array}{ccc}{\sigma }_2^2+{\sigma }_1^2& {\sigma }_2^2+{\sigma }_1^2& {\sigma }_2^2+{\sigma }_1^2\\ {\sigma }_1^2+{\sigma }_3^2& {\sigma }_1^2+{\sigma }_3^2& {\sigma }_1^2+{\sigma }_3^2\\ {\sigma }_1^2+{\sigma }_4^2& {\sigma }_1^2+{\sigma }_4^2& {\sigma }_1^2+{\sigma }_4^2\end{array}\right]$$

其余流程相同，不再推导。