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

2024-06-11 08:36

本文主要是介绍让GNSSRTK不再难【第二天-第7部分2】,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

状态更新计算过程:
  1. 计算卡尔曼增益
    根据预测的误差协方差矩阵 P k − P_k^- Pk 和观测噪声协方差矩阵 R R R 计算卡尔曼增益 K k K_k Kk
    K k = P k − H T ( H P k − H T + R ) − 1 K_k = P_k^- H^T (H P_k^- H^T + R)^{-1} Kk=PkHT(HPkHT+R)1

    带入预测的 P k − P_k^- Pk R R R 计算:

    P k − = [ C o v X X 0 0 0 0 0 0 C o v Y Y 0 0 0 0 0 0 C o v Z Z 0 0 0 0 0 0 C o v δ t δ t 0 0 0 0 0 0 ∗ ∗ 0 0 0 0 ∗ ∗ ] P_k^- = \begin{bmatrix} Cov_{XX} & 0 & 0 & 0 & 0 & 0 \\ 0 & Cov_{YY} & 0 & 0 & 0 & 0 \\ 0 & 0 & Cov_{ZZ} & 0 & 0 & 0 \\ 0 & 0 & 0 & Cov_{\delta t \delta t} & 0 & 0 \\ 0 & 0 & 0 & 0 & * & * \\ 0 & 0 & 0 & 0 & * & * \\ \end{bmatrix} Pk= CovXX000000CovYY000000CovZZ000000Covδtδt0000000000

    R = [ σ 1 2 0 ⋯ 0 0 σ 2 2 ⋯ 0 ⋮ ⋮ ⋱ ⋮ 0 0 ⋯ σ n 2 ] R = \begin{bmatrix} \sigma_1^2 & 0 & \cdots & 0 \\ 0 & \sigma_2^2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & \sigma_n^2 \\ \end{bmatrix} R= σ12000σ22000σn2

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

    A = [ 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 ] A = \begin{bmatrix} 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ l_{f_n}^{C_n} & m_{f_n}^{C_n} & n_{f_n}^{C_n} & -1 & 0 & -1 & 0 \end{bmatrix} A= lf1G1lf2G2lf3G3lfnGnlf1C1lf2C2lf3C3lfnCnmf1G1mf2G2mf3G3mfnGnmf1C1mf2C2mf3C3mfnCnnf1G1nf2G2nf3G3nfnGnnf1C1nf2C2nf3C3nfnCn11111111000000000000111100000000

    则卡尔曼增益 K k K_k Kk 计算为:
    K k = P k − A T ( A P k − A T + R ) − 1 K_k = P_k^- A^T (A P_k^- A^T + R)^{-1} Kk=PkAT(APkAT+R)1

    (1)计算 A P k − A T A P_k^- A^T APkAT
    A P k − A T = A [ C o v X X 0 0 0 0 0 0 C o v Y Y 0 0 0 0 0 0 C o v Z Z 0 0 0 0 0 0 C o v δ t δ t 0 0 0 0 0 0 ∗ ∗ 0 0 0 0 ∗ ∗ ] A T A P_k^- A^T = A \begin{bmatrix} Cov_{XX} & 0 & 0 & 0 & 0 & 0 \\ 0 & Cov_{YY} & 0 & 0 & 0 & 0 \\ 0 & 0 & Cov_{ZZ} & 0 & 0 & 0 \\ 0 & 0 & 0 & Cov_{\delta t \delta t} & 0 & 0 \\ 0 & 0 & 0 & 0 & * & * \\ 0 & 0 & 0 & 0 & * & * \\ \end{bmatrix} A^T APkAT=A CovXX000000CovYY000000CovZZ000000Covδtδt0000000000 AT

    (2)计算 A P k − A T + R A P_k^- A^T + R APkAT+R
    A P k − A T + R = A [ C o v X X 0 0 0 0 0 0 C o v Y Y 0 0 0 0 0 0 C o v Z Z 0 0 0 0 0 0 C o v δ t δ t 0 0 0 0 0 0 ∗ ∗ 0 0 0 0 ∗ ∗ ] A T + R A P_k^- A^T + R = A \begin{bmatrix} Cov_{XX} & 0 & 0 & 0 & 0 & 0 \\ 0 & Cov_{YY} & 0 & 0 & 0 & 0 \\ 0 & 0 & Cov_{ZZ} & 0 & 0 & 0 \\ 0 & 0 & 0 & Cov_{\delta t \delta t} & 0 & 0 \\ 0 & 0 & 0 & 0 & * & * \\ 0 & 0 & 0 & 0 & * & * \\ \end{bmatrix} A^T + R APkAT+R=A CovXX000000CovYY000000CovZZ000000Covδtδt0000000000 AT+R

    由于 A P k − A T + R A P_k^- A^T + R APkAT+R 是对角矩阵,其逆矩阵为:
    ( A P k − A T + R ) − 1 = [ ( C o v X X + σ 1 2 ) − 1 0 0 0 ( C o v Y Y + σ 2 2 ) − 1 0 0 0 ( C o v Z Z + σ 3 2 ) − 1 ] (A P_k^- A^T + R)^{-1} = \begin{bmatrix} (Cov_{XX} + \sigma_1^2)^{-1} & 0 & 0 \\ 0 & (Cov_{YY} + \sigma_2^2)^{-1} & 0 \\ 0 & 0 & (Cov_{ZZ} + \sigma_3^2)^{-1} \\ \end{bmatrix} (APkAT+R)1= (CovXX+σ12)1000(CovYY+σ22)1000(CovZZ+σ32)1

    (3)计算 K k K_k Kk
    K k = P k − A T ( A P k − A T + R ) − 1 K_k = P_k^- A^T (A P_k^- A^T + R)^{-1} Kk=PkAT(APkAT+R)1

    带入 P k − P_k^- Pk ( A P k − A T + R ) − 1 (A P_k^- A^T + R)^{-1} (APkAT+R)1
    K k = [ C o v X X 0 0 0 C o v Y Y 0 0 0 C o v Z Z 0 0 0 0 0 0 0 0 0 0 0 0 ] A T [ ( C o v X X + σ 1 2 ) − 1 0 0 0 ( C o v Y Y + σ 2 2 ) − 1 0 0 0 ( C o v Z Z + σ 3 2 ) − 1 ] K_k = \begin{bmatrix} Cov_{XX} & 0 & 0 \\ 0 & Cov_{YY} & 0 \\ 0 & 0 & Cov_{ZZ} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} A^T \begin{bmatrix} (Cov_{XX} + \sigma_1^2)^{-1} & 0 & 0 \\ 0 & (Cov_{YY} + \sigma_2^2)^{-1} & 0 \\ 0 & 0 & (Cov_{ZZ} + \sigma_3^2)^{-1} \\ \end{bmatrix} Kk= CovXX0000000CovYY0000000CovZZ0000 AT (CovXX+σ12)1000(CovYY+σ22)1000(CovZZ+σ32)1

    简化计算得到:
    K k = [ C o v X X ( C o v X X + σ 1 2 ) − 1 0 0 0 C o v Y Y ( C o v Y Y + σ 2 2 ) − 1 0 0 0 C o v Z Z ( C o v Z Z + σ 3 2 ) − 1 0 0 0 0 0 0 0 0 0 0 0 0 ] K_k = \begin{bmatrix} Cov_{XX} (Cov_{XX} + \sigma_1^2)^{-1} & 0 & 0 \\ 0 & Cov_{YY} (Cov_{YY} + \sigma_2^2)^{-1} & 0 \\ 0 & 0 & Cov_{ZZ} (Cov_{ZZ} + \sigma_3^2)^{-1} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} Kk= CovXX(CovXX+σ12)10000000CovYY(CovYY+σ22)10000000CovZZ(CovZZ+σ32)10000

    因此,卡尔曼增益 K k K_k Kk 为:
    K k = [ C o v X X C o v X X + σ 1 2 0 0 0 C o v Y Y C o v Y Y + σ 2 2 0 0 0 C o v Z Z C o v Z Z + σ 3 2 0 0 0 0 0 0 0 0 0 0 0 0 ] K_k = \begin{bmatrix} \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2} & 0 & 0 \\ 0 & \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2} & 0 \\ 0 & 0 & \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} Kk= CovXX+σ12CovXX0000000CovYY+σ22CovYY0000000CovZZ+σ32CovZZ0000

  2. 更新状态估计
    根据观测值 z k z_k zk 和预测值 x ^ k − \hat{x}_k^- x^k 进行状态更新:
    x k = x ^ k − + K k ( z k − H x ^ k − ) x_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) xk=x^k+Kk(zkHx^k)

    带入观测值 z k z_k zk 和预测值 x ^ k − \hat{x}_k^- x^k

    假设 x ^ k − \hat{x}_k^- x^k 为:
    x ^ k − = [ x ^ k , 1 − x ^ k , 2 − x ^ k , 3 − ⋮ x ^ k , 7 − ] \hat{x}_k^- = \begin{bmatrix} \hat{x}_{k,1}^- \\ \hat{x}_{k,2}^- \\ \hat{x}_{k,3}^- \\ \vdots \\ \hat{x}_{k,7}^- \end{bmatrix} x^k= x^k,1x^k,2x^k,3x^k,7

    观测值 z k z_k zk 为:
    z k = [ z k , 1 z k , 2 z k , 3 ] z_k = \begin{bmatrix} z_{k,1} \\ z_{k,2} \\ z_{k,3} \end{bmatrix} zk= zk,1zk,2zk,3

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

    A = [ 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 ] A = \begin{bmatrix} 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ l_{f_n}^{C_n} & m_{f_n}^{C_n} & n_{f_n}^{C_n} & -1 & 0 & -1 & 0 \end{bmatrix} A= lf1G1lf2G2lf3G3lfnGnlf1C1lf2C2lf3C3lfnCnmf1G1mf2G2mf3G3mfnGnmf1C1mf2C2mf3C3mfnCnnf1G1nf2G2nf3G3nfnGnnf1C1nf2C2nf3C3nfnCn11111111000000000000111100000000

    则状态更新为:

    x k = [ x ^ k , 1 − x ^ k , 2 − x ^ k , 3 − ⋮ x ^ k , 7 − ] + K k ( [ z k , 1 z k , 2 z k , 3 ] − A [ x ^ k , 1 − x ^ k , 2 − x ^ k , 3 − ⋮ x ^ k , 7 − ] ) x_k = \begin{bmatrix} \hat{x}_{k,1}^- \\ \hat{x}_{k,2}^- \\ \hat{x}_{k,3}^- \\ \vdots \\ \hat{x}_{k,7}^- \end{bmatrix}+ K_k \left( \begin{bmatrix} z_{k,1} \\ z_{k,2} \\ z_{k,3} \end{bmatrix} - A \begin{bmatrix} \hat{x}_{k,1}^- \\ \hat{x}_{k,2}^- \\ \hat{x}_{k,3}^- \\ \vdots \\ \hat{x}_{k,7}^- \end{bmatrix} \right) xk= x^k,1x^k,2x^k,3x^k,7 +Kk zk,1zk,2zk,3 A x^k,1x^k,2x^k,3x^k,7

    简化后:
    x k = [ x ^ k , 1 − x ^ k , 2 − x ^ k , 3 − ⋮ x ^ k , 7 − ] + K k [ z k , 1 − ( A x ^ k − ) 1 z k , 2 − ( A x ^ k − ) 2 z k , 3 − ( A x ^ k − ) 3 ] x_k = \begin{bmatrix} \hat{x}_{k,1}^- \\ \hat{x}_{k,2}^- \\ \hat{x}_{k,3}^- \\ \vdots \\ \hat{x}_{k,7}^- \end{bmatrix}+ K_k \begin{bmatrix} 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{bmatrix} xk= x^k,1x^k,2x^k,3x^k,7 +Kk zk,1(Ax^k)1zk,2(Ax^k)2zk,3(Ax^k)3

    带入卡尔曼增益 K k K_k Kk 计算结果:
    K k = [ C o v X X C o v X X + σ 1 2 0 0 0 C o v Y Y C o v Y Y + σ 2 2 0 0 0 C o v Z Z C o v Z Z + σ 3 2 0 0 0 0 0 0 0 0 0 0 0 0 ] K_k = \begin{bmatrix} \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2} & 0 & 0 \\ 0 & \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2} & 0 \\ 0 & 0 & \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} Kk= CovXX+σ12CovXX0000000CovYY+σ22CovYY0000000CovZZ+σ32CovZZ0000

    最终状态更新为:
    x k = [ x ^ k , 1 − x ^ k , 2 − x ^ k , 3 − ⋮ x ^ k , 7 − ] + [ C o v X X C o v X X + σ 1 2 0 0 0 C o v Y Y C o v Y Y + σ 2 2 0 0 0 C o v Z Z C o v Z Z + σ 3 2 0 0 0 0 0 0 0 0 0 0 0 0 ] [ z k , 1 − ( A x ^ k − ) 1 z k , 2 − ( A x ^ k − ) 2 z k , 3 − ( A x ^ k − ) 3 ] x_k = \begin{bmatrix} \hat{x}_{k,1}^- \\ \hat{x}_{k,2}^- \\ \hat{x}_{k,3}^- \\ \vdots \\ \hat{x}_{k,7}^- \end{bmatrix}+ \begin{bmatrix} \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2} & 0 & 0 \\ 0 & \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2} & 0 \\ 0 & 0 & \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} 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{bmatrix} xk= x^k,1x^k,2x^k,3x^k,7 + CovXX+σ12CovXX0000000CovYY+σ22CovYY0000000CovZZ+σ32CovZZ0000 zk,1(Ax^k)1zk,2(Ax^k)2zk,3(Ax^k)3

  3. 更新误差协方差矩阵
    更新误差协方差矩阵 P k P_k Pk

    P k = ( I − K k A ) P k − P_k = (I - K_k A) P_k^- Pk=(IKkA)Pk

    带入计算:

    假设 I I I 为单位矩阵:
    I = [ 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 ] I = \begin{bmatrix} 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{bmatrix} I= 1000000010000000100000001000000010000000100000001

    卡尔曼增益 K k K_k Kk 为:
    K k = [ C o v X X C o v X X + σ 1 2 0 0 0 C o v Y Y C o v Y Y + σ 2 2 0 0 0 C o v Z Z C o v Z Z + σ 3 2 0 0 0 0 0 0 0 0 0 0 0 0 ] K_k = \begin{bmatrix} \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2} & 0 & 0 \\ 0 & \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2} & 0 \\ 0 & 0 & \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} Kk= CovXX+σ12CovXX0000000CovYY+σ22CovYY0000000CovZZ+σ32CovZZ0000

    假设观测矩阵 H H H 为设计矩阵 A A A
    A = [ 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 ] A = \begin{bmatrix} 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ l_{f_n}^{C_n} & m_{f_n}^{C_n} & n_{f_n}^{C_n} & -1 & 0 & -1 & 0 \end{bmatrix} A= lf1G1lf2G2lf3G3lfnGnlf1C1lf2C2lf3C3lfnCnmf1G1mf2G2mf3G3mfnGnmf1C1mf2C2mf3C3mfnCnnf1G1nf2G2nf3G3nfnGnnf1C1nf2C2nf3C3nfnCn11111111000000000000111100000000

    则更新误差协方差矩阵为:
    P k = ( [ 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 ] − [ C o v X X C o v X X + σ 1 2 0 0 0 C o v Y Y C o v Y Y + σ 2 2 0 0 0 C o v Z Z C o v Z Z + σ 3 2 0 0 0 0 0 0 0 0 0 0 0 0 ] [ 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 ] ) [ C o v X X 0 0 0 0 0 0 C o v Y Y 0 0 0 0 0 0 C o v Z Z 0 0 0 0 0 0 C o v δ t δ t 0 0 0 0 0 0 ∗ ∗ 0 0 0 0 ∗ ∗ ] P_k = \left( \begin{bmatrix} 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{bmatrix} - \begin{bmatrix} \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2} & 0 & 0 \\ 0 & \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2} & 0 \\ 0 & 0 & \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ 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 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ l_{f_n}^{C_n} & m_{f_n}^{C_n} & n_{f_n}^{C_n} & -1 & 0 & -1 & 0 \end{bmatrix} \right) \begin{bmatrix} Cov_{XX} & 0 & 0 & 0 & 0 & 0 \\ 0 & Cov_{YY} & 0 & 0 & 0 & 0 \\ 0 & 0 & Cov_{ZZ} & 0 & 0 & 0 \\ 0 & 0 & 0 & Cov_{\delta t \delta t} & 0 & 0 \\ 0 & 0 & 0 & 0 & * & * \\ 0 & 0 & 0 & 0 & * & * \\ \end{bmatrix} Pk= 1000000010000000100000001000000010000000100000001 CovXX+σ12CovXX0000000CovYY+σ22CovYY0000000CovZZ+σ32CovZZ0000 lf1G1lf2G2lf3G3lfnGnlf1C1lf2C2lf3C3lfnCnmf1G1mf2G2mf3G3mfnGnmf1C1mf2C2mf3C3mfnCnnf1G1nf2G2nf3G3nfnGnnf1C1nf2C2nf3C3nfnCn11111111000000000000111100000000 CovXX000000CovYY000000CovZZ000000Covδtδt0000000000

    进一步计算得到:进一步计算得到:
    P k = ( [ 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 ] − [ C o v X X C o v X X + σ 1 2 0 0 0 C o v Y Y C o v Y Y + σ 2 2 0 0 0 C o v Z Z C o v Z Z + σ 3 2 0 0 0 0 0 0 0 0 0 0 0 0 ] ) [ C o v X X 0 0 0 0 0 0 C o v Y Y 0 0 0 0 0 0 C o v Z Z 0 0 0 0 0 0 C o v δ t δ t 0 0 0 0 0 0 ∗ ∗ 0 0 0 0 ∗ ∗ ] P_k = \left( \begin{bmatrix} 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{bmatrix} - \begin{bmatrix} \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2} & 0 & 0 \\ 0 & \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2} & 0 \\ 0 & 0 & \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2} \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \right) \begin{bmatrix} Cov_{XX} & 0 & 0 & 0 & 0 & 0 \\ 0 & Cov_{YY} & 0 & 0 & 0 & 0 \\ 0 & 0 & Cov_{ZZ} & 0 & 0 & 0 \\ 0 & 0 & 0 & Cov_{\delta t \delta t} & 0 & 0 \\ 0 & 0 & 0 & 0 & * & * \\ 0 & 0 & 0 & 0 & * & * \\ \end{bmatrix} Pk= 1000000010000000100000001000000010000000100000001 CovXX+σ12CovXX0000000CovYY+σ22CovYY0000000CovZZ+σ32CovZZ0000 CovXX000000CovYY000000CovZZ000000Covδtδt0000000000

    最终得到:
    P k = [ ( 1 − C o v X X C o v X X + σ 1 2 ) C o v X X 0 0 0 0 0 0 ( 1 − C o v Y Y C o v Y Y + σ 2 2 ) C o v Y Y 0 0 0 0 0 0 ( 1 − C o v Z Z C o v Z Z + σ 3 2 ) C o v Z Z 0 0 0 0 0 0 C o v δ t δ t 0 0 0 0 0 0 ∗ ∗ 0 0 0 0 ∗ ∗ ] P_k = \begin{bmatrix} \left(1 - \frac{Cov_{XX}}{Cov_{XX} + \sigma_1^2}\right) Cov_{XX} & 0 & 0 & 0 & 0 & 0 \\ 0 & \left(1 - \frac{Cov_{YY}}{Cov_{YY} + \sigma_2^2}\right) Cov_{YY} & 0 & 0 & 0 & 0 \\ 0 & 0 & \left(1 - \frac{Cov_{ZZ}}{Cov_{ZZ} + \sigma_3^2}\right) Cov_{ZZ} & 0 & 0 & 0 \\ 0 & 0 & 0 & Cov_{\delta t \delta t} & 0 & 0 \\ 0 & 0 & 0 & 0 & * & * \\ 0 & 0 & 0 & 0 & * & * \\ \end{bmatrix} Pk= (1CovXX+σ12CovXX)CovXX000000(1CovYY+σ22CovYY)CovYY000000(1CovZZ+σ32CovZZ)CovZZ000000Covδtδt0000000000

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

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

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

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

对于没有做星间单差之前

V u d = [ p 1 p 2 p 3 p 4 ] R u d = [ σ 1 2 0 0 0 0 σ 2 2 0 0 0 0 σ 3 2 0 0 0 0 σ 4 2 ] V_{ud} = \begin{bmatrix} p^1 \\ p^2 \\ p^3 \\ p^4 \end{bmatrix} \quad R_{ud} = \begin{bmatrix} \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{bmatrix} Vud= p1p2p3p4 Rud= σ120000σ220000σ320000σ42

星间单差之后

V s d = [ p 2 − p 1 p 3 − p 1 p 4 − p 1 ] R s d = [ σ 2 2 + σ 1 2 σ 2 2 + σ 1 2 σ 2 2 + σ 1 2 σ 1 2 + σ 3 2 σ 1 2 + σ 3 2 σ 1 2 + σ 3 2 σ 1 2 + σ 4 2 σ 1 2 + σ 4 2 σ 1 2 + σ 4 2 ] V_{sd} = \begin{bmatrix} p^2 - p^1 \\ p^3 - p^1 \\ p^4 - p^1 \end{bmatrix} \quad R_{sd} = \begin{bmatrix} \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{bmatrix} Vsd= p2p1p3p1p4p1 Rsd= σ22+σ12σ12+σ32σ12+σ42σ22+σ12σ12+σ32σ12+σ42σ22+σ12σ12+σ32σ12+σ42

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

这篇关于让GNSSRTK不再难【第二天-第7部分2】的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1050673

相关文章

poj 2976 分数规划二分贪心(部分对总体的贡献度) poj 3111

poj 2976: 题意: 在n场考试中,每场考试共有b题,答对的题目有a题。 允许去掉k场考试,求能达到的最高正确率是多少。 解析: 假设已知准确率为x,则每场考试对于准确率的贡献值为: a - b * x,将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码: #include <iostream>#include <cstdio>#incl

负债不再是障碍?银行信贷“白名单“揭秘

谈及银行信贷产品,常闻有言称存在无需考量负债与查询记录之奇品,此等说法十有八九为中介诱人上钩之辞。轻信之下,恐将步入连环陷阱。除非个人资质出类拔萃,如就职于国央企或事业单位,工龄逾年,五险一金完备,还款能力卓越,或能偶遇线下产品对查询记录稍显宽容,然亦非全然无视。宣称全然不顾者,纯属无稽之谈。 银行非慈善机构,不轻易于困境中援手,更偏爱锦上添花之举。若无坚实资质,即便求助于银行亦难获青睐。反

笔记整理—内核!启动!—kernel部分(2)从汇编阶段到start_kernel

kernel起始与ENTRY(stext),和uboot一样,都是从汇编阶段开始的,因为对于kernel而言,还没进行栈的维护,所以无法使用c语言。_HEAD定义了后面代码属于段名为.head .text的段。         内核起始部分代码被解压代码调用,前面关于uboot的文章中有提到过(eg:zImage)。uboot启动是无条件的,只要代码的位置对,上电就工作,kern

Java基础回顾系列-第二天-面向对象编程

面向对象编程 Java类核心开发结构面向对象封装继承多态 抽象类abstract接口interface抽象类与接口的区别深入分析类与对象内存分析 继承extends重写(Override)与重载(Overload)重写(Override)重载(Overload)重写与重载之间的区别总结 this关键字static关键字static变量static方法static代码块 代码块String类特

项目实战系列三: 家居购项目 第四部分

购物车 🌳购物车🍆显示购物车🍆更改商品数量🍆清空购物车&&删除商品 🌳生成订单 🌳购物车 需求分析 1.会员登陆后, 可以添加家居到购物车 2.完成购物车的设计和实现 3.每添加一个家居,购物车的数量+1, 并显示 程序框架图 1.新建src/com/zzw/furns/entity/CartItem.java, CartItem-家居项模型 /***

码蹄集部分题目(2024OJ赛9.4-9.8;线段树+树状数组)

1🐋🐋配对最小值(王者;树状数组) 时间限制:1秒 占用内存:64M 🐟题目思路 MT3065 配对最小值_哔哩哔哩_bilibili 🐟代码 #include<bits/stdc++.h> using namespace std;const int N=1e5+7;int a[N],b[N],c[N],n,q;struct QUERY{int l,r,id;}que

关于断言的部分用法

1、带变量的断言  systemVerilog assertion 中variable delay的使用,##[variable],带变量的延时(可变延时)_assertion中的延时-CSDN博客 2、until 的使用 systemVerilog assertion 中until的使用_verilog until-CSDN博客 3、throughout的使用   常用于断言和假设中的

牛客小白月赛100部分题解

比赛地址:牛客小白月赛100_ACM/NOI/CSP/CCPC/ICPC算法编程高难度练习赛_牛客竞赛OJ A.ACM中的A题 #include<bits/stdc++.h>using namespace std;#define ll long long#define ull = unsigned long longvoid solve() {ll a,b,c;cin>>a>>b>

VB和51单片机串口通信讲解(只针对VB部分)

标记:该篇文章全部搬自如下网址:http://www.crystalradio.cn/thread-321839-1-1.html,谢谢啦            里面关于中文接收的部分,大家可以好好学习下,题主也在研究中................... Commport;设置或返回串口号。 SettingS:以字符串的形式设置或返回串口通信参数。 Portopen:设置或返回串口