基于改进扩展卡尔曼滤波的GNSS单点动态定位方法

摘要

本发明涉及一种基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，属于卫星导航技术领域。本方法将GNSS接收机的位置速度、钟差与钟漂作为位置参数设为状态向量X_k，通过状态转移方程，由前一历元的状态向量推算当前历元状态向量的预测值；并通过观测方程进一步获取状态向量预测值的修正量；将预测值和修正量加权，获取状态向量的估计值。在基于扩展卡曼滤波定位过程中，本方法通过延迟对状态向量误差协方差矩阵的更新，使得在初始取状态向量的情况下，滤波估计值快速收敛在真值附近，并达到很高的定位测速精度；不需要保存每一步的计算数据，占用计算机内存资源少；适用于GNSS单点动态定位。

主权项

1.基于改进扩展卡尔曼滤波的GNSS单点动态定位方法，其特征在于：步骤1：在观测历元数k=1时，初始状态向量初始状态向量误差协方差矩阵P₀＝diag[1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_p 1/ε_v 1/ε_a 1/ε_t 1/ε_f]；ECEF坐标系下，GNSS接收机的状态向量定义为：$X_k=[x_k^u,v_k^{\mathrm{ux}},a_k^{\mathrm{ux}},y_k^u,v_k^{\mathrm{uy}},a_k^{\mathrm{uy}},z_k^u,v_k^{\mathrm{uz}},a_k^{\mathrm{uz}},\mathrm{c\Delta }{t}_k^u,\mathrm{c\Delta }{f}_k^u]$其中，k为观测历元数，分别为ECEF坐标下GNSS接收机载体的三维位置、速度和加速度，和分别为接收机的钟差和钟漂；X_k的滤波估计为：${\hat{X}}_k=[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},\mathrm{c\Delta }{\hat{t}}_k^u,\mathrm{c\Delta }{\hat{f}}_k^u{]}^T$ε_p为载体与地心距离平方的倒数，ε_v为载体最大允许速度平方的倒数，ε_a为载体最大允许加速度的倒数，ε_t为接收机钟差与光速乘积平方的倒数，ε_f为接收机钟漂与光速乘积平方的倒数；步骤2：计算状态向量X_k的预测值${\hat{X}}_{k|k-1}={\Phi }_{k|k-1}{\hat{X}}_{k-1}=[{\hat{x}}_{k|k-1}^u,{\hat{v}}_{k|k-1,}^{\mathrm{ux}}{\hat{a}}_{k|k-1}^{\mathrm{ux}},{\hat{y}}_{k|k-1}^u,{\hat{v}}_{k|k-1,}^{\mathrm{uy}}{\hat{a}}_{k|k-1}^{\mathrm{uy}},{\hat{z}}_{k|k-1}^u,{\hat{v}}_{k|k-1}^{\mathrm{uz}},{\hat{a}}_{k|k-1}^{\mathrm{uz}},\mathrm{c\Delta }{\hat{t}}_{k|k-1}^u,\mathrm{c\Delta }{\hat{f}}_{k|k-1}^u{]}^T$其中Φ_k|k-1为状态转移矩阵，来自接收机载体的动力学模型，${\Phi }_{k|k-1}=\left[\begin{array}{cccc}{\Phi }_{k|k-1}^x& & & \\ & {\Phi }_{k|k-1}^y& & \\ & & {\Phi }_{k|k-1}^z& \\ & & & {\Phi }_{k|k-1}^c\end{array}\right]$分别是ECEF坐标系下，X向，Y向和Z向运动状态量的状态转移矩阵，依据动力学模型确定；是有关钟差和钟漂的状态转移矩阵，依据时钟模型确定；步骤3：计算预测值的协方差矩阵$P_{k|k-1}={\Phi }_{k|k-1}{P}_{k-1}{\Phi }_{k|k-1}^T+Q_{k-1}$过程噪声协方差矩阵：$Q_{k-1}=\left[\begin{array}{cccc}2{\alpha }_x{\sigma }_x^2{Q}_{k-1}^x& & & \\ & 2{\alpha }_y{\mathrm{\sigma \delta }}_y^2{Q}_{k-1}^y& & \\ & & 2{\alpha }_z{\sigma }_z^2{Q}_{k-1}^z& \\ & & & Q_{k-1}^c\end{array}\right]$和分别是ECEF坐标系下，X,Y,Z三向运动状态量的过程噪声矩阵；α_y和α_z分别为X向，Y向和Z向的机动加速度频率；分别是X,Y,Z三向的机动加速度方差；步骤4：根据步骤3得到的P_k|k-1，计算滤波增益矩阵K_k：$K_k=P_{k|k-1}{H}_k^T[H_k{P}_{k|k-1}{H}_k^T+Q_k{]}^{-1}$(a)观测矩阵H_k由线性化而来，和分别为第s颗可见星的伪距方程[6]和多普勒方程[7]，s＝1,...,n，n为接收机观测到的参与定位计算的可见星总数；$R_k^s=\sqrt{{(X_k^s-x_k^u)}^2+{(Y_k^s-x_k^u)}^2+{(Z_k^s-z_k^u)}^2}+c·\Delta t_k^u---\left[6\right]$$D_k^s=[(X_k^s-x_k^u)·(V_k^{\mathrm{sx}}-v_k^{\mathrm{ux}})+(Y_k^s-y_k^u)·(V_k^{\mathrm{sy}}-v_k^{\mathrm{uy}})+(Z_k^s-z_k^u)·(V_k^{\mathrm{sz}}-v_k^{\mathrm{uz}})]/{\rho }_k^s+c·\Delta f_k^u---\left[7\right]$其中，和分别为ECEF坐标系下第s颗可见星在第k个历元的三维位置和速度，是第s颗可见星与GNSS接收机之间的真实距离，${\rho }_k^s=\sqrt{{(X_k^s-x_k^u)}^2+{(Y_k^s-y_k^u)}^2+{(Z_k^s-z_k^u)}^2};$$H_k=\left[\begin{array}{cccccccc}{h}_{1x|k}^1& 0& h_{1y|k}^1& 0& h_{1z|k}^1& 0& 1& 0\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ h_{\mathrm{nx}|k}^1& 0& h_{\mathrm{ny}|k}^1& 0& h_{\mathrm{nz}|k}^1& 0& 1& 0\\ h_{1x|k}^2& h_{1x|k}^1& h_{1y|k}^2& h_{1y|k}^1& h_{1y|k}^2& h_{1z|k}^1& 0& 0\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .\\ h_{\mathrm{nx}|k}^2& h_{\mathrm{nx}|k}^1& h_{\mathrm{ny}|k}^2& h_{\mathrm{ny}|k}^1& h_{\mathrm{nz}|k}^2& h_{\mathrm{nz}|k}^1& 0& 1\end{array}\right]---\left[8\right]$H_k中前n行由伪距方程线性化而来，后n行由多普勒方程线性化而来；$h_{\mathrm{sx}|k}^1=\frac{{\partial R}_k^s}{{\partial x}_k^u}=\frac{{\partial D}_k^s}{{\partial v}_k^{\mathrm{ux}}}=\frac{({\hat{x}}_{k|k-1}^u-X_k^s)}{{\rho }_{k|k-1}^s}$$h_{\mathrm{sx}|k}^2=\frac{{\partial D}_k^s}{{\partial x}_k^u}=\frac{({\hat{v}}_{k|k-1}^{\mathrm{ux}}-V_k^{\mathrm{sx}}){\left({\rho }_{k|k-1}^s\right)}^2-({\hat{x}}_{k|k-1}^u-X_k^s)J_{k|k-1}^s}{{\left({\rho }_{k|k-1}^s\right)}^2}$${\rho }_{k|k-1}^s=\sqrt{{(X_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Y_k^s-{\hat{y}}_{k|k-1}^u)}^2+{(Z_k^s-{\hat{z}}_{k|k-1}^u)}^2}$$J_{k|k-1}^s=(X_k^s-{\hat{x}}_{k|k-1}^u)·(V_k^{\mathrm{sx}}-{\hat{v}}_{k|k-1}^{\mathrm{ux}})+(Y_k^s-{\hat{y}}_{k|k-1}^u)·(V_k^{\mathrm{sy}}-{\hat{v}}_{k|k-1}^{\mathrm{uy}})+(Z_k^s-{\hat{z}}_{k|k-1}^u)·(V_k^{\mathrm{sz}}-{\hat{v}}_{k|k-1}^{\mathrm{uz}})$用y和z分别代替和中的x，得到和和(b)过程噪声方差矩阵$O_k=\left[\begin{array}{cc}{\beta }_1·I_{n\times n}& 0\\ 0& {\beta }_2·I_{n\times n}\end{array}\right]---\left[9\right]$其中，I_n×n为n阶方阵，β₁为伪距观测噪声方差，β₂为多普勒频移观测噪声方差；步骤5：根据步骤2得到的计算状态向量X_k的滤波估计值${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k·\Delta Z_k$$\Delta Z_k=[(r_k^1-{\hat{r}}_{k|k-1}^1),...,(r_k^n-{\hat{r}}_{k|k-1}^n),(d_k^1-{\hat{d}}_{k|k-1}^1),...,(d_k^n-{\hat{d}}_{k|k-1}^n)]---\left[10\right]$和分别为第s颗可见星的伪距观测值和多普勒观测值；${\hat{r}}_{k|k-1}^s=\sqrt{{(X_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Y_k^s-{\hat{x}}_{k|k-1}^u)}^2+{(Z_k^s-{\hat{z}}_{k|k-1}^u)}^2}+c·\Delta {\hat{t}}_{k|k-1}^u,$${\hat{d}}_{k|k-1}^s=[(X_k^s-{\hat{x}}_{k|k-1}^u)·(V_k^{\mathrm{sx}}-{\hat{v}}_{k|k-1}^{\mathrm{ux}})+(Y_k^s-{\hat{y}}_{k|k-1}^u)·(V_k^{\mathrm{sy}}-v_{k|k-1}^{\mathrm{uy}})+(Z_k^s-z_{k|k-1}^u)·(V_k^{\mathrm{sz}}-v_{k|k-1}^{\mathrm{uz}})]/{\rho }_{k|k-1}^s+c·\Delta {\hat{f}}_{k|k-1}^u$步骤6：判断k的取值，若k≤m，转到步骤7，若k＞m，转到步骤8；步骤7：令P_k=P_k-1，执行步骤9；步骤8：根据步骤4得到的K_k，计算P_k：P_k＝[I-K_kH_k]P_k|k-1，执行步骤9；步骤9：输出${\hat{X}}_k=[{\hat{x}}_k^u,{\hat{v}}_k^{\mathrm{ux}},{\hat{a}}_k^{\mathrm{ux}},{\hat{y}}_k^u,{\hat{v}}_k^{\mathrm{uy}},{\hat{a}}_k^{\mathrm{uy}},{\hat{z}}_k^u,{\hat{v}}_k^{\mathrm{uz}},{\hat{a}}_k^{\mathrm{uz}},c{\Delta \hat{t}}_k^u,\mathrm{c\Delta }{\hat{f}}_k^u{]}^T;$即为第k个历元的定位和测速结果；步骤10：令k=k+1，转入步骤2，继续计算并输出下一个历元的状态向量的滤波估计值，直到不需要进行接收机载体的定位测速时，停止计算。