CN105277960A - 基于改进自适应滤波算法的高超目标导航信号跟踪方法 - Google Patents

Abstract

本发明公开了一种基于改进自适应滤波算法的高超目标导航信号跟踪方法。高超声速目标接收到的GPS信号存在较大的多普勒频移，传统的基于锁相环跟踪环路无法进行跟踪，针对这一问题，采用改进的自适应卡尔曼滤波算法，通过实时递推的方法估计出每个时刻目标接收到GPS信号的多普勒频移和码相位偏移，然后将其用于GPS信号的解调、解扩，最终可以得到导航数据。再由导航数据计算出目标到卫星的伪距及卫星位置，便可以对目标位置进行解算。

Description

基于改进自适应滤波算法的高超目标导航信号跟踪方法

技术领域

本发明属于GPS定位导航领域，具体是一种基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法。

背景技术

GPS由于能够为用户提供全天候位置信息服务，并随着航空、航天技术的迅猛发展，其在军事上得到了广泛应用，高超声速环境下GPS系统的应用也日益受到人们的关注。GPS系统所有权、控制权和运行权都属于美国，在军事、航空航天等敏感领域，为保障其国家安全和维护其军事强国的地位，国外的高性能产品和高新技术都是对我国实行封锁和限制的，而随着国防现代化建设的进行、国防科技日新月异的发展形势，突破这种技术壁垒刻不容缓。高超声速环境下目标接收到的GPS信号具有较大的多普勒频移，传统的基于锁相环的跟踪环路无法进行跟踪，解决这一问题目前主要由三种方法。一是锁频环(FLL)辅助锁相环(PLL)进行跟踪，需要根据经验设定一个阈值在FLL和PLL之间进行切换来实现对高超声速目标GPS信号的跟踪，这类方案较为复杂并缺乏灵活性；二是使用惯性系统进行辅助跟踪，虽然基本可以满足应用要求，但是成本较高，并且结构也较为复杂；三是采用参数估计的方法进行跟踪，基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法即是此类方法。

文献(SunF,ZhangHQ.ApplicationofanewAdaptiveKalmanFiliteringalgorithmininitialalignmentofINS[J].IEEEInternationalConferenceonMechatronics&Automation,2011:2312-2316.)中提出一种基于简化的Sage-Husa自适应卡尔曼滤波组合导航GPS信号跟踪方法，该方法利用简化的Sage-Husa自适应卡尔曼滤波算法对GPS信号的多普勒频移和码相位进行实时递推估计，但该方法也存在缺点：只对观测噪声的统计特性进行实时在线估计，而假设过程噪声的统计特性已知，当过程噪声的统计特性偏差较大时，滤波效果会变差。

发明内容

本发明的目的在于提供一种基于改进自适应滤波算法的高超目标导航信号跟踪方法，该方法能够对高超声速目标接收到的GPS信号进行跟踪，可以对高超声速目标进行定位并对其运动轨迹进行估计。

实现本发明目的的技术方案为：一种基于改进自适应滤波算法的高超目标导航信号跟踪方法，步骤如下：

第一步，设高超声速目标接收到的编号为i卫星的GPS信号为：s_i(t)＝C((1+η)(t-τ_i))D((1+η)(t-τ_i))cos2π(f_IF+f_di)t+n_i(t)，式中τ_i表示时间延迟，f_IF为信号中频频率，f_di多普勒频移，η＝f_di/f_L1，f_L1＝1575.42MHz为GPS信号载频，n_i(t)为噪声项，C(t)为C/A码，D(t)为导航数据码；

第二步，建立GPS信号跟踪的卡尔曼滤波模型，其中状态方程为： $\left[\begin{array}{c}{x}_{p,k}\\ x_{\mathrm{\ω},k}\\ x_{a,k}\\ x_{t,k}\end{array}\right]=\left[\begin{array}{cccc}1& \mathrm{\Δ}T& \frac{{\mathrm{\ΔT}}^2}{2}& 0\\ 0& 1& \mathrm{\Δ}T& 0\\ 0& 0& 1& 0\\ 0& \frac{\mathrm{\Δ}T}{2\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{4\mathrm{\π}\×1540}& 1\end{array}\right]\left[\begin{array}{c}{x}_{p,k-1}\\ x_{\mathrm{\ω},k-1}\\ x_{a,k-1}\\ x_{t,k-1}\end{array}\right]+\left[\begin{array}{cc}-\mathrm{\Δ}T& 0\\ 0& 0\\ 0& 0\\ 0& -\mathrm{\Δ}T\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]+{\[I\]}_{4\×4}{W}_{k-1},$ 式中X_k＝[x_p,kx_ω,kx_a,kx_t,k]^T为状态变量； ${\mathrm{\Φ}}_{k,k-1}=\left[\begin{array}{cccc}1& \mathrm{\Δ}T& \frac{{\mathrm{\ΔT}}^2}{2}& 0\\ 0& 1& \mathrm{\Δ}T& 0\\ 0& 0& 1& 0\\ 0& \frac{\mathrm{\Δ}T}{2\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{4\mathrm{\π}\×1540}& 1\end{array}\right]$ 为状态转换矩阵， ${\mathrm{\Ψ}}_{k,k-1}=\left[\begin{array}{cc}-\mathrm{\Δ}T& 0\\ 0& 0\\ 0& 0\\ 0& -\mathrm{\Δ}T\end{array}\right]$ 为输入控制矩阵， $u_{k,k-1}=\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]$ 为输入控制变量，Γ_k,k-1＝[I]_4×4为过程噪声控制矩阵，W_k-1为过程噪声矩阵，x_p为真实载波和本地复现载波相位差，x_ω是载波的多普勒漂移；x_a是载波的多普勒漂移变化率，x_t为码相位偏差，ΔT为相邻两次处理时间间隔，ω_NCO,k-1是本地复现载波的角频率偏移在ω_IF基础上的差，f_ca,k-1是本地复现码传输速率在1.023MHz基础上的差；观测方程为： $\left[\begin{array}{c}\mathrm{\Δ}{\widetilde{\mathrm{\φ}}}_k\\ \mathrm{\Δ}{\widetilde{\mathrm{\τ}}}_k\end{array}\right]=\left[\begin{array}{cccc}1& \frac{\mathrm{\Δ}T}{2}& \frac{{\mathrm{\ΔT}}^2}{6}& 0\\ 0& \frac{\mathrm{\Δ}T}{4\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{12\mathrm{\π}\×1540}& 1\end{array}\right]\left[\begin{array}{c}{x}_p\\ x_{\mathrm{\ω}}\\ x_a\\ x_t\end{array}\right]+\frac{-{\mathrm{\ΔT}}_k}{2}\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]+V_k,$ 式中 $Z_k=\left[\begin{array}{c}\mathrm{\Δ}{\widetilde{\mathrm{\φ}}}_k\\ \mathrm{\Δ}{\widetilde{\mathrm{\τ}}}_k\end{array}\right]$ 为观测变量，来自鉴相器的输出， $H_k=\left[\begin{array}{cccc}1& \frac{\mathrm{\Δ}T}{2}& \frac{{\mathrm{\ΔT}}^2}{6}& 0\\ 0& \frac{\mathrm{\Δ}T}{4\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{12\mathrm{\π}\×1540}& 1\end{array}\right]$ 为观测矩阵， $M_k=\frac{-{\mathrm{\ΔT}}_k}{2}\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]$ 为观测误差向量，V_k为观测误差矩阵；

第三步，采用基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法对第一步中的GPS信号进行处理，估计出其中的多普勒频移f_di和码相位偏差，由码相位偏差得到时延τ_i，然后对GPS信号进行解调解扩得到导航数据。

第三步所述对第一步中的GPS信号进行处理的步骤为：

3.1，建立GPS信号卡尔曼滤波模型，状态方程：X_k＝Φ_k,k-1X_k-1+Ψ_k,k-1u_k,k-1+Γ_k,k-1W_k-1，观测方程：Z_k＝H_kX_k+M_k+V_k。

3.2，初始化，k＝1，误差方差矩阵P₀＝I_4×4，状态变量 ${\hat{X}}_0={\left[\begin{array}{cccc}0& {\widehat{\mathrm{\ω}}}_{IF}& 0& 0\end{array}\right]}^T;$

3.3，系统状态变量预估计 ${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\mathrm{\Ψ}}_{k,k-1}{u}_{k-1}+{\hat{q}}_{k-1},$ 式中为过程噪声均值；

3.4，预估计误差方差 $P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T,$ 式中Q_k-1为过程噪声方差矩阵；

3.5，计算新息 ${\tilde{Z}}_k=Z_k-M_k-H_k{\hat{X}}_{k,k-1}-\hat{r},$ 式中为观测噪声均值；

3.6，根据滤波最优原则计算遗忘因子式中R_k为观测噪声方差矩阵；

3.7，估计观测噪声统计特性， ${\hat{r}}_k=(1-d_{k-1}){\hat{r}}_{k-1}+d_{k-1}(Z_k-H_k{\hat{X}}_{k,k-1}-M_k),$ ${\hat{R}}_k=(1-d_{k-1}){\hat{R}}_{k-1}+d_{k-1}({\tilde{Z}}_k{\tilde{Z}}_k^T-H_k{P}_{k,k-1}{H}_k^T),$ 式中d_k＝(1-b)/(1-b^k+1)；

3.8，计算卡尔曼滤波增益矩阵 $K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1};$

3.9，对状态变量预估计进行校正， ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-M_k-H_k{\hat{X}}_{k,k-1});$

3.10，估计过程噪声统计特性， ${\hat{q}}_k=(1-d_k){\hat{q}}_{k-1}+d_{k-1}({\hat{X}}_k-{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}-{\mathrm{\Ψ}}_{k,k-1}{u}_{k-1}),$ ${\hat{Q}}_k=(1-d_{k-1}){\hat{Q}}_{k-1}+d_{k-1}(K_k{\tilde{Z}}_k{\tilde{Z}}_k^T{K}_k^T+P_k-{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T),$ 并估计误差方差阵，P_k＝[1-K_kH_k]P_k,k-1；

3.11，更新载频和CA码传输速率，将估计出来的代入Z_k＝H_kX_k+M_k，可以得到Z_k的最优估计，利用Z_k更新载频和CA码传输速率， 其中ka、kb为可调参数，这里分别取0.6和0.9。判断跟踪过程是否结束，结束就退出循环，否则k＝k+1，返回步骤3.1。

本发明与现有的基于卡尔曼GPS信号跟踪方法相比，其显著优点为：本发明方法能够对噪声的统计特性进行在线实时估计，具有较强的鲁棒性。特别是当噪声的统计特性未知或偏差较大时，该方法具有更好的定位精度。

附图说明

图1为超高声速目标接收到的GPS仿真信号。

图2为解析基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪的实验结果。(a)编号2号卫星跟踪结果，(b)编号2号卫星跟踪结果局部放大效果图，(c)编号2号卡尔曼滤波输出，(d)定位误差，(e)目标运动轨迹。

具体实施方式

下面结合附图对本发明作进一步详细描述。

本发明为基于改进自适应滤波算法的高超目标导航信号跟踪方法。本发明主要作用在于超高声速目标GPS信号跟踪，具体实施步骤如下：

第一步，设高超声速目标接收到的编号为i卫星的GPS信号为：s_i(t)＝C((1+η)(t-τ_i))D((1+η)(t-τ_i))cos2π(f_IF+f_di)t+n_i(t)，式中τ_i表示时间延迟，f_IF为信号中频频率，f_di多普勒频移，η＝f_di/f_L1，f_L1＝1575.42MHz为GPS信号载频，n_i(t)为噪声项，C(t)为C/A码，D(t)为导航数据码，图1给出GPS信号。

第二步，建立GPS信号跟踪的卡尔曼滤波模型，其中状态方程为: $\left[\begin{array}{c}{x}_{p,k}\\ x_{\mathrm{\ω},k}\\ x_{a,k}\\ x_{t,k}\end{array}\right]=\left[\begin{array}{cccc}1& \mathrm{\Δ}T& \frac{{\mathrm{\ΔT}}^2}{2}& 0\\ 0& 1& \mathrm{\Δ}T& 0\\ 0& 0& 1& 0\\ 0& \frac{\mathrm{\Δ}T}{2\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{4\mathrm{\π}\×1540}& 1\end{array}\right]\left[\begin{array}{c}{x}_{p,k-1}\\ x_{\mathrm{\ω},k-1}\\ x_{a,k-1}\\ x_{t,k-1}\end{array}\right]+\left[\begin{array}{cc}-\mathrm{\Δ}T& 0\\ 0& 0\\ 0& 0\\ 0& -\mathrm{\Δ}T\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]+{\[I\]}_{4\×4}{W}_{k-1},$ 式中X_k＝[x_p,kx_ω,kx_a,kx_t,k]^T为状态变量， ${\mathrm{\Φ}}_{k,k-1}=\left[\begin{array}{cccc}1& \mathrm{\Δ}T& \frac{{\mathrm{\ΔT}}^2}{2}& 0\\ 0& 1& \mathrm{\Δ}T& 0\\ 0& 0& 1& 0\\ 0& \frac{\mathrm{\Δ}T}{2\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{4\mathrm{\π}\×1540}& 1\end{array}\right]$ 为状态转换矩阵， ${\mathrm{\Ψ}}_{k,k-1}=\left[\begin{array}{cc}-\mathrm{\Δ}T& 0\\ 0& 0\\ 0& 0\\ 0& -\mathrm{\Δ}T\end{array}\right]$ 为输入控制矩阵， $u_{k,k-1}=\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]$ 为输入控制变量，Γ_k,k-1＝[I]_4×4为过程噪声控制矩阵，W_k-1为过程噪声矩阵，x_p为真实载波和本地复现载波相位差，x_ω是载波的多普勒漂移；x_a是载波的多普勒漂移变化率，x_t为码相位偏差，ΔT为相邻两次处理时间间隔，ω_NCO,k-1是本地复现载波的角频率偏移在ω_IF基础上的差，f_ca,k-1是本地复现码传输速率在1.023MHz基础上的差；观测方程为： $\left[\begin{array}{c}\mathrm{\Δ}{\widetilde{\mathrm{\φ}}}_k\\ \mathrm{\Δ}{\widetilde{\mathrm{\τ}}}_k\end{array}\right]=\left[\begin{array}{cccc}1& \frac{\mathrm{\Δ}T}{2}& \frac{{\mathrm{\ΔT}}^2}{6}& 0\\ 0& \frac{\mathrm{\Δ}T}{4\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{12\mathrm{\π}\×1540}& 1\end{array}\right]\left[\begin{array}{c}{x}_p\\ x_{\mathrm{\ω}}\\ x_a\\ x_t\end{array}\right]+\frac{-{\mathrm{\ΔT}}_k}{2}\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]+V_k,$ 式中 $Z_k=\left[\begin{array}{c}\mathrm{\Δ}{\widetilde{\mathrm{\φ}}}_k\\ \mathrm{\Δ}{\widetilde{\mathrm{\τ}}}_k\end{array}\right]$ 为观测变量，来自鉴相器的输出。 $H_k=\left[\begin{array}{cccc}1& \frac{\mathrm{\Δ}T}{2}& \frac{{\mathrm{\ΔT}}^2}{6}& 0\\ 0& \frac{\mathrm{\Δ}T}{4\mathrm{\π}\×1540}& \frac{{\mathrm{\ΔT}}^2}{12\mathrm{\π}\×1540}& 1\end{array}\right]$ 为观测矩阵， $M_k=\frac{-{\mathrm{\ΔT}}_k}{2}\left[\begin{array}{c}{\mathrm{\ω}}_{NCO,k-1}\\ f_{ca,k-1}\end{array}\right]$ 为观测误差向量，V_k为观测误差矩阵。

第三步，建立了GPS信号跟踪的卡尔曼滤波模型后，采用基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法对第一步中的GPS信号进行处理，估计出其中的多普勒频移f_di和码相位偏差，有码相位偏差可以得到时延τ_i，然后对GPS信号进行解调解扩得到导航数据。

GPS信号进行处理的具体步骤如下：

第一步，初始化，k＝1，误差方差矩阵P₀＝I_4×4，状态变量 ${\hat{X}}_0={\left[\begin{array}{cccc}0& {\widehat{\mathrm{\ω}}}_{IF}& 0& 0\end{array}\right]}^T;$

第二步，系统状态变量预估计 ${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+{\mathrm{\Ψ}}_{k,k-1}{u}_{k-1}+{\hat{q}}_{k-1},$ 式中为过程噪声均值；

第三步，预估计误差方差 $P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T,$ 式中Q_k-1为过程噪声方差矩阵；

第四步，计算新息 ${\tilde{Z}}_k=Z_k-M_k-H_k{\hat{X}}_{k,k-1}-\hat{r},$ 式中为观测噪声均值；

第五步，根据滤波最优原则计算遗忘因子式中R_k为观测噪声方差矩阵；

第六步，估计观测噪声统计特性， ${\hat{r}}_k=(1-d_{k-1}){\hat{r}}_{k-1}+d_{k-1}(Z_k-H_k{\hat{X}}_{k,k-1}-M_k),$ ${\hat{R}}_k=(1-d_{k-1}){\hat{R}}_{k-1}+d_{k-1}({\tilde{Z}}_k{\tilde{Z}}_k^T-H_k{P}_{k,k-1}{H}_k^T),$ 式中d_k＝(1-b)/(1-b^k+1)；

第七步，计算卡尔曼滤波增益矩阵 $K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1};$

第八步，对状态变量预估计进行校正， ${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-M_k-H_k{\hat{X}}_{k,k-1});$

第九步，估计过程噪声统计特性， ${\hat{q}}_k=(1-d_k){\hat{q}}_{k-1}+d_{k-1}({\hat{X}}_k-{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}-{\mathrm{\Ψ}}_{k,k-1}{u}_{k-1}),$ ${\hat{Q}}_k=(1-d_{k-1}){\hat{Q}}_{k-1}+d_{k-1}(K_k{\tilde{Z}}_k{\tilde{Z}}_k^T{K}_k^T+P_k-{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T),$ 并估计误差方差阵，P_k＝[1-K_kH_k]P_k,k-1；

第十步，更新载频和CA码传输速率，将估计出来的代入Z_k＝H_kX_k+M_k，可以得到Z_k的最优估计，利用Z_k更新载频和CA码传输速率， 其中ka、kb为可调参数，这里分别取0.6和0.9。判断跟踪过程是否结束，结束就退出循环，否则k＝k+1，返回第一步。

为了验证本发明的正确性与有效性，进行以下仿真实验：目标初始运动速度设为15马赫，沿着曲线运动，然后在25s时失锁，后在30s时重新接收到信号。采用软件模拟该运动目标接收到的GPS信号。首先，根据实施步骤中的第二步对运动目标接收到的GPS信号进行处理，跟踪结果(编号2号卫星)如图2(a)和图2(b)所示，其中蓝色实线为实际导航数据，红色虚线为跟踪结果，可见跟踪结果和实际导航数据是吻合的。为进一步比较基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法的精度，图2(c)给出了卡尔曼滤波估计得到的GPS信号多普勒频移和码相位偏移，可以看出估计得到的与实际的GPS信号多普勒频移误差很小，估计得到的码相位误差也较小。接下来再从定位精度进一步验证基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法的有效性，如图2(d所示，给出了定位误差。最后在图2(e)中给出了目标的运动轨迹。

Claims (2)

1.一种基于改进自适应滤波算法的高超目标导航信号跟踪方法，其特征在于步骤如下：

第一步，设高超声速目标接收到的编号为i卫星的GPS信号为：s_i(t)＝C((1+η)(t-τ_i))D((1+η)(t-τ_i))cos2π(f_IF+f_di)t+n_i(t)，式中τ_i表示时间延迟，f_IF为信号中频频率，f_di多普勒频移，η＝f_di/f_L1，f_L1＝1575.42MHz为GPS信号载频，n_i(t)为噪声项，C(t)为C/A码，D(t)为导航数据码；

第二步，建立GPS信号跟踪的卡尔曼滤波模型，其中状态方程为：式中X_k＝[x_p,kx_ω,kx_a,kx_t,k]^T为状态变量；为状态转换矩阵，为输入控制矩阵，为输入控制变量，Γ_k,k-1＝[I]_4×4为过程噪声控制矩阵，W_k-1为过程噪声矩阵，x_p为真实载波和本地复现载波相位差，x_ω是载波的多普勒漂移；x_a是载波的多普勒漂移变化率，x_t为码相位偏差，ΔT为相邻两次处理时间间隔，ω_NCO,k-1是本地复现载波的角频率偏移在ω_IF基础上的差，f_ca,k-1是本地复现码传输速率在1.023MHz基础上的差；观测方程为：式中为观测变量，来自鉴相器的输出，为观测矩阵，为观测误差向量，V_k为观测误差矩阵；

第三步，采用基于改进自适应卡尔曼滤波算法的高超声速目标GPS信号跟踪方法对第一步中的GPS信号进行处理，估计出其中的多普勒频移f_di和码相位偏差，由码相位偏差得到时延τ_i，然后对GPS信号进行解调解扩得到导航数据。

2.根据权利要求1所述的基于改进自适应滤波算法的高超目标导航信号跟踪方法，其特征在于：第三步所述对GPS信号进行处理的具体步骤如下：

3.1，初始化，k＝1，误差方差矩阵P₀＝I_4×4，状态变量

3.2，系统状态变量预估计式中为过程噪声均值；

3.3，预估计误差方差式中Q_k-1为过程噪声W_k的协方差矩阵；

3.4，计算新息式中为观测噪声均值；

3.5，根据滤波最优原则计算遗忘因子且0＜b＜1，式中R_k为观测噪声V_k的协方差矩阵；

3.6，估计观测噪声统计特性， 式中d_k＝(1-b)/(1-b^k+1)；

3.7，计算卡尔曼滤波增益矩阵

3.8，对状态变量预估计进行校正，

3.9，估计过程噪声统计特性， 并估计误差方差阵，P_k＝[1-K_kH_k]P_k,k-1；

3.10，更新载频和CA码传输速率，将估计出来的代入Z_k＝H_kX_k+M_k，得到Z_k的最优估计，利用Z_k更新载频和CA码传输速率， 其中ka、kb为可调参数；判断跟踪过程是否结束，结束就退出循环，否则k＝k+1，返回步骤3.1。