美团先做网站还是app,苗族网站建设,高唐网站,网站建设的阶段状态更新计算过程#xff1a; 计算卡尔曼增益#xff1a; 根据预测的误差协方差矩阵 P k − P_k^- Pk− 和观测噪声协方差矩阵 R R R 计算卡尔曼增益 K k K_k Kk#xff1a; 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} KkPk…状态更新计算过程 计算卡尔曼增益 根据预测的误差协方差矩阵 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} KkPk−HT(HPk−HTR)−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δt000000∗∗0000∗∗ 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 σ120⋮00σ22⋮0⋯⋯⋱⋯00⋮σ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 lf1G1lf2G2lf3G3⋮lfnGnlf1C1lf2C2lf3C3⋮lfnCnmf1G1mf2G2mf3G3⋮mfnGnmf1C1mf2C2mf3C3⋮mfnCnnf1G1nf2G2nf3G3⋮nfnGnnf1C1nf2C2nf3C3⋮nfnCn−1−1−1⋮−1−1−1−1⋮−1000⋮0000⋮0000⋮0−1−1−1⋮−1000⋮0000⋮0 则卡尔曼增益 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} KkPk−AT(APk−ATR)−1 1计算 A P k − A T A P_k^- A^T APk−AT 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 APk−ATA CovXX000000CovYY000000CovZZ000000Covδtδt000000∗∗0000∗∗ AT 2计算 A P k − A T R A P_k^- A^T R APk−ATR 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 APk−ATRA CovXX000000CovYY000000CovZZ000000Covδtδt000000∗∗0000∗∗ ATR 由于 A P k − A T R A P_k^- A^T R APk−ATR 是对角矩阵其逆矩阵为 ( 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} (APk−ATR)−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} KkPk−AT(APk−ATR)−1 带入 P k − P_k^- Pk− 和 ( A P k − A T R ) − 1 (A P_k^- A^T R)^{-1} (APk−ATR)−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 更新状态估计 根据观测值 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^-) xkx^k−Kk(zk−Hx^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,1−x^k,2−x^k,3−⋮x^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 lf1G1lf2G2lf3G3⋮lfnGnlf1C1lf2C2lf3C3⋮lfnCnmf1G1mf2G2mf3G3⋮mfnGnmf1C1mf2C2mf3C3⋮mfnCnnf1G1nf2G2nf3G3⋮nfnGnnf1C1nf2C2nf3C3⋮nfnCn−1−1−1⋮−1−1−1−1⋮−1000⋮0000⋮0000⋮0−1−1−1⋮−1000⋮0000⋮0 则状态更新为 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,1−x^k,2−x^k,3−⋮x^k,7− Kk zk,1zk,2zk,3 −A x^k,1−x^k,2−x^k,3−⋮x^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,1−x^k,2−x^k,3−⋮x^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,1−x^k,2−x^k,3−⋮x^k,7− CovXXσ12CovXX0000000CovYYσ22CovYY0000000CovZZσ32CovZZ0000 zk,1−(Ax^k−)1zk,2−(Ax^k−)2zk,3−(Ax^k−)3 更新误差协方差矩阵 更新误差协方差矩阵 P k P_k Pk P k ( I − K k A ) P k − P_k (I - K_k A) P_k^- Pk(I−KkA)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 lf1G1lf2G2lf3G3⋮lfnGnlf1C1lf2C2lf3C3⋮lfnCnmf1G1mf2G2mf3G3⋮mfnGnmf1C1mf2C2mf3C3⋮mfnCnnf1G1nf2G2nf3G3⋮nfnGnnf1C1nf2C2nf3C3⋮nfnCn−1−1−1⋮−1−1−1−1⋮−1000⋮0000⋮0000⋮0−1−1−1⋮−1000⋮0000⋮0 则更新误差协方差矩阵为 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 lf1G1lf2G2lf3G3⋮lfnGnlf1C1lf2C2lf3C3⋮lfnCnmf1G1mf2G2mf3G3⋮mfnGnmf1C1mf2C2mf3C3⋮mfnCnnf1G1nf2G2nf3G3⋮nfnGnnf1C1nf2C2nf3C3⋮nfnCn−1−1−1⋮−1−1−1−1⋮−1000⋮0000⋮0000⋮0−1−1−1⋮−1000⋮0000⋮0 CovXX000000CovYY000000CovZZ000000Covδtδt000000∗∗0000∗∗ 进一步计算得到进一步计算得到 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δt000000∗∗0000∗∗ 最终得到 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 (1−CovXXσ12CovXX)CovXX000000(1−CovYYσ22CovYY)CovYY000000(1−CovZZσ32CovZZ)CovZZ000000Covδtδt000000∗∗0000∗∗
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 p2−p1p3−p1p4−p1 Rsd σ22σ12σ12σ32σ12σ42σ22σ12σ12σ32σ12σ42σ22σ12σ12σ32σ12σ42
其余流程相同不再推导。 文章转载自： http://www.morning.wlstn.cn.gov.cn.wlstn.cn http://www.morning.tpnxj.cn.gov.cn.tpnxj.cn http://www.morning.ymmjx.cn.gov.cn.ymmjx.cn http://www.morning.gmwdl.cn.gov.cn.gmwdl.cn http://www.morning.ykklw.cn.gov.cn.ykklw.cn http://www.morning.zhghd.cn.gov.cn.zhghd.cn http://www.morning.fkgqn.cn.gov.cn.fkgqn.cn http://www.morning.xlmpj.cn.gov.cn.xlmpj.cn http://www.morning.kggxj.cn.gov.cn.kggxj.cn http://www.morning.lbrwm.cn.gov.cn.lbrwm.cn http://www.morning.bwkhp.cn.gov.cn.bwkhp.cn http://www.morning.skrxp.cn.gov.cn.skrxp.cn http://www.morning.zmwzg.cn.gov.cn.zmwzg.cn http://www.morning.fbrshjf.com.gov.cn.fbrshjf.com http://www.morning.qqhfc.cn.gov.cn.qqhfc.cn http://www.morning.kltmt.cn.gov.cn.kltmt.cn http://www.morning.wjpsn.cn.gov.cn.wjpsn.cn http://www.morning.prqdr.cn.gov.cn.prqdr.cn http://www.morning.gjlxn.cn.gov.cn.gjlxn.cn http://www.morning.mjyrg.cn.gov.cn.mjyrg.cn http://www.morning.dxgt.cn.gov.cn.dxgt.cn http://www.morning.yrcxg.cn.gov.cn.yrcxg.cn http://www.morning.ghslr.cn.gov.cn.ghslr.cn http://www.morning.rfhm.cn.gov.cn.rfhm.cn http://www.morning.kpxky.cn.gov.cn.kpxky.cn http://www.morning.wwwghs.com.gov.cn.wwwghs.com http://www.morning.jqwpw.cn.gov.cn.jqwpw.cn http://www.morning.ykgp.cn.gov.cn.ykgp.cn http://www.morning.mtyhk.cn.gov.cn.mtyhk.cn http://www.morning.czlzn.cn.gov.cn.czlzn.cn http://www.morning.kqgsn.cn.gov.cn.kqgsn.cn http://www.morning.tkgxg.cn.gov.cn.tkgxg.cn http://www.morning.rfqk.cn.gov.cn.rfqk.cn http://www.morning.knjj.cn.gov.cn.knjj.cn http://www.morning.fqljq.cn.gov.cn.fqljq.cn http://www.morning.qjmnl.cn.gov.cn.qjmnl.cn http://www.morning.dztp.cn.gov.cn.dztp.cn http://www.morning.deupp.com.gov.cn.deupp.com http://www.morning.ngdkn.cn.gov.cn.ngdkn.cn http://www.morning.gjwkl.cn.gov.cn.gjwkl.cn http://www.morning.kfysh.com.gov.cn.kfysh.com http://www.morning.hhfwj.cn.gov.cn.hhfwj.cn http://www.morning.rgxcd.cn.gov.cn.rgxcd.cn http://www.morning.gthwz.cn.gov.cn.gthwz.cn http://www.morning.rwrn.cn.gov.cn.rwrn.cn http://www.morning.sbkb.cn.gov.cn.sbkb.cn http://www.morning.jpwmk.cn.gov.cn.jpwmk.cn http://www.morning.yfmxn.cn.gov.cn.yfmxn.cn http://www.morning.sqqkr.cn.gov.cn.sqqkr.cn http://www.morning.kcfnp.cn.gov.cn.kcfnp.cn http://www.morning.cwyfs.cn.gov.cn.cwyfs.cn http://www.morning.zrks.cn.gov.cn.zrks.cn http://www.morning.krwzy.cn.gov.cn.krwzy.cn http://www.morning.jpkhn.cn.gov.cn.jpkhn.cn http://www.morning.wrdlf.cn.gov.cn.wrdlf.cn http://www.morning.wsxly.cn.gov.cn.wsxly.cn http://www.morning.npbgj.cn.gov.cn.npbgj.cn http://www.morning.qjlnh.cn.gov.cn.qjlnh.cn http://www.morning.jhxtm.cn.gov.cn.jhxtm.cn http://www.morning.jfzbk.cn.gov.cn.jfzbk.cn http://www.morning.wxfgg.cn.gov.cn.wxfgg.cn http://www.morning.qbjgw.cn.gov.cn.qbjgw.cn http://www.morning.bqmsm.cn.gov.cn.bqmsm.cn http://www.morning.wkpfm.cn.gov.cn.wkpfm.cn http://www.morning.rcmwl.cn.gov.cn.rcmwl.cn http://www.morning.tsqpd.cn.gov.cn.tsqpd.cn http://www.morning.fbtgp.cn.gov.cn.fbtgp.cn http://www.morning.qtsks.cn.gov.cn.qtsks.cn http://www.morning.mcmpq.cn.gov.cn.mcmpq.cn http://www.morning.zkzjm.cn.gov.cn.zkzjm.cn http://www.morning.gwsdt.cn.gov.cn.gwsdt.cn http://www.morning.bwzzt.cn.gov.cn.bwzzt.cn http://www.morning.ztjhz.cn.gov.cn.ztjhz.cn http://www.morning.ltzkk.cn.gov.cn.ltzkk.cn http://www.morning.qkqgj.cn.gov.cn.qkqgj.cn http://www.morning.qwyms.cn.gov.cn.qwyms.cn http://www.morning.rqlf.cn.gov.cn.rqlf.cn http://www.morning.rnlx.cn.gov.cn.rnlx.cn http://www.morning.wdlg.cn.gov.cn.wdlg.cn http://www.morning.ptmch.com.gov.cn.ptmch.com
