CN102323602A - 一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法 - Google Patents

Abstract

本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法，该自适应二阶卡尔曼滤波器的载波跟踪环路包括本地载波发生器、积分清除器、载波环鉴相器、环路滤波器和自适应二阶卡尔曼滤波器。本发明克服了传统卡尔曼滤波器在先验信息不准确时使滤波器发散，从而导致跟踪环路失锁的缺点。本发明在系统模型不完整或其先验信息不准确时，通过渐消因子自适应的调节预测误差协方差矩阵，从而达到实时调整增益矩阵的目的，使滤波器接近最优，可以有效的抑制滤波器的发散，且本发明中渐消因子的估算方法计算简单，计算量小，适应性强，提高了滤波算法的可靠性。

Description

一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法

技术领域

本发明属于GPS基带信号处理技术领域，具体涉及一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法。

背景技术

近年来，随着航空、航天技术的迅猛发展以及其在军事上的广泛应用，高动态环境下GPS系统的应用日益受到人们的关注。迄今为止，国外不断将高动态GPS的相关产品投入到实际的战争应用中。如战斧巡航导弹和斯拉姆导弹等都采用了高动态GPS接收机进行制导，其优越的可靠性和高精度性使美国在海湾战争和科索沃战争中获得了巨大的军事效应。目前，GPS系统的高动态应用领域仍然在不断拓展，美国的大部分军用飞机都将装备高动态GPS接收机，一些主要的精确制导弹药都将通过GPS制导来瞄准。因此，高动态环境下卫星定位导航系统的合理有效利用对于现代战争的成败将会起到关键性的作用。

而在高动态环境下，由于载体的高速运动使接收信号载波多普勒频移随时间变化的非常快。此时，若采用普通的GPS接收机，载波跟踪环路带宽要能大到足够容忍这种动态引起的载波频率和相位的正常波动，以保证环路对信号的持续跟踪。然而，环路带宽越宽，越多频率成分的噪声被允许进入环路，则环路的噪声性能越差，信号跟踪就越不准确，因此传统的锁相环是不能直接运用到高动态接收机上的。为了解决上述问题，近年来，一些研究者提出了使用卡尔曼滤波器或者扩展卡尔曼滤波器的载波跟踪环路，在系统模型建立准确和噪声统计特性已知的前提下，经过滤波器的递推可以获得较为精确的载波相位估计值，从而提高跟踪精度。然而在载体运动条件比较复杂的环境下，有可能不能准确的获得必要的先验信息而致使滤波器发散，从而导致跟踪环路失锁。

二阶卡尔曼滤波器最先是由Friedland提出解决线性系统的状态模型存在常量偏差时的估计方法，是一种并行算法，可以有效降低滤波器的计算量和增加滤波器的数值稳定性。最近有研究者将其推广到系统状态方程偏差项为随机量时的状态估计问题。但是当系统模型和偏差模型的噪声统计特性不准确时，往往会导致二阶卡尔曼滤波器发散。

发明内容

本发明针对现有技术中存在的高动态环境下GPS载波跟踪环存在的问题，提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法。本发明克服了传统卡尔曼滤波器在先验信息不准确时使滤波器发散，从而导致跟踪环路失锁的缺点。

本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路，包括本地载波发生器、积分清除器、载波环鉴相器、环路滤波器和自适应二阶卡尔曼滤波器。

数字中频信号分别与本地载波发生器复现的载波及90°相移后的载波相乘后，再分别通过积分清除器提高载噪比，然后分别将每个积分清除器的相干积分结果均输出至载波环鉴相器，利用积分清除器输出的相干积分结果估算当前跟踪环路的载波相位差异，再将载波环鉴相器的输出载波相位差异作为自适二阶卡尔曼滤波器的观测值，经附带偏差的载波相位动态系统模型的自适应二阶卡尔曼滤波器递推出相位差异的估计值，相位差异的估计值输出至环路滤波器，经环路滤波器输出的控制信号使本地载波发生器实时调整。

本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路的滤波方法，包括自适应无偏差滤波和自适应偏差滤波两部分，具体包括以下几个步骤：

步骤一：定义自适应二阶卡尔曼滤波的初始状态以及计算过程中的中间变量：

定义初始状态为：

$V_0=P_0^{\mathrm{xb}}{\left(P_0^b\right)}^{-1},$ $x_0(+)=x_0^{*}-V_0{b}_0^{*}---\left(4\right)$

$b_0(+)=b_0^{*},$ $P_0^b(+)=P_0^b---\left(5\right)$

$P_0^x(+)=P_0^x-V_0{P}_0^b{V}_0^T---\left(6\right)$

其中和

分别表示自适应二阶卡尔曼滤波初始状态向量x₀的均值和协方差，

和

分别表示自适应二阶卡尔曼滤波初始偏差向量b₀的均值和协方差，

表示x₀和b₀的互协方差矩阵，V₀，为定义的混合矩阵；x₀(+)、b₀(+)、

分别表示自适应无偏差滤波的初始状态估计值、自适应偏差滤波的初始状态估计值、自适应偏差滤波的初始估计均方误差矩阵、自适应无偏差滤波的初始估计均方误差矩阵。

定义中间变量：

N_k＝H_kU_k+D_k， $V_k=U_k-{K_k^{x}N}_k---\left(7\right)$

$U_k={\stackrel{\‾}{U}}_k[I-{\mathrm{\λ}}_k^b{Q}_{k-1}^b{\left[P_k^b(-)\right]}^{-1}]---\left(8\right)$

${\stackrel{\‾}{U}}_k=({\mathrm{\Φ}}_{k-1}{V}_{k-1}+B_{k-1})A_{k-1}^{-1}---\left(9\right)$

$u_k=({\stackrel{\‾}{U}}_{k+1}-U_{k+1})A_k{b}_k(+)---\left(10\right)$

${\stackrel{\‾}{Q}}_k^x=Q_k^x+U_{k+1}{Q}_k^b{\stackrel{\‾}{U}}_{k+1}---\left(11\right)$

其中H_k为观测矩阵，

为自适应偏差滤波的滤波增益矩阵，I为单位矩阵，

为自适应偏差滤波的渐消因子，

表示系统噪声协方差，

和

表示偏差系统噪声协方差，

为自适应偏差滤波的预测均方误差矩阵，Φ_k-1为状态转移矩阵，A_k-1，B_k-1，D_k为随机偏差系数矩阵，u_k，U_k，V_k，N_k，

为定义的混合矩阵；b_k(+)表示自适应偏差滤波的状态估计值。

步骤二：计算自适应无偏差滤波过程中的状态一步预测值x_k(-)：

                         x_k(-)＝Φ_k-1x_k-1(+)+u_k-1                        (12)

其中，Φ_k-1为状态转移矩阵，x_k-1(+)为状态估计值，u_k-1为步骤一中定义的混合矩阵。

步骤三：计算自适应无偏差滤波方法中的预测均方误差矩阵

$P_k^x(-)={\mathrm{\λ}}_k^x({\mathrm{\Φ}}_{k-1}{P}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}^x)---\left(13\right)$

其中，

为估计均方误差矩阵，

为系统噪声协方差，

为自适应无偏差滤波的渐消因子。

步骤四：计算自适应无偏差滤波的滤波增益矩阵

$K_k^x=P_k^x(-)H_k^T{[H_k{P}_k^x(-)H_K^T+R_k]}^{-1}---\left(14\right)$

其中，R_k为观测噪声协方差。

步骤五：计算自适应无偏差滤波的估计均方误差矩阵

$P_k^x(+)=(I-K_k^x{H}_k)P_k^x(-)---\left(15\right)$

步骤六：计算自适应无偏差滤波的渐消因子

(1)计算k时刻观测量z_k的新息序列

${\mathrm{\η}}_k^x=Z_k-H_k{x}_k(-)---\left(16\right)$

其中，H_k为观测矩阵，x_k(-)为自适应无偏差滤波的状态一步预测值。

(2)计算k时刻新息序列协方差

的理论值：

t_k时刻自适应无偏差滤波的预测均方误差协方差矩阵

为

$P_k^x(-)={\mathrm{\λ}}_k^x({\mathrm{\Φ}}_{k-1}{P}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}^x)---\left(17\right)$

其中Φ_k-1为状态转移矩阵，自适应无偏差滤波的为估计均方误差矩阵，

为系统噪声的协方差，t_k为时间量，新息序列协方差

的理论值为：

$C_k^x=E\left[{\mathrm{\η}}_k^x{\left({\mathrm{\η}}_k^x\right)}^T\right]=H_k{P}_k^x(-)H_k^T+R_k---\left(18\right)$

其中E[·]表示求解协方差矩阵，R_k为量测噪声方差阵。

(3)计算k时刻新息序列协方差的估计值

当滤波增益矩阵

为最优增益矩阵时，新息序列

是白噪声序列，则新息序列的自相关函数等于零：

$E\left[{\mathrm{\η}}_{k+j}^x{\left({\mathrm{\η}}_k^x\right)}^T\right]=P_k^x(-)H_k^T-K_k^x{C}_k^x=0---\left(19\right)$

计算新息序列协方差的估计值

${\stackrel{\‾}{C}}_k^x=\left\{\begin{array}{cc}\frac{{\mathrm{\λ}}_{k-1}^x{\mathrm{\η}}_k^x{\left({\mathrm{\η}}_k^x\right)}^T}{1+{\mathrm{\λ}}_{k-1}^x},& k>1\\ \frac{1}{2}{\mathrm{\η}}_0^x{\left({\mathrm{\η}}_0^x\right)}^T,& k=1\end{array}\right.---\left(20\right)$

其中，

为k＝0时的新息序列，

为k时刻的新息序列，

为k-1时刻的渐消因子。

(4)计算自适应无偏差滤波的渐消因子

设预测均方误差协方差的估计值

和新息序列协方差的估计值

分别为：

${\stackrel{\‾}{P}}_k^x(-)={\mathrm{\λ}}_k^x{P}_k^x(-),$ ${\stackrel{\‾}{C}}_k^x={\mathrm{\α}}_k^x{C}_k^x---\left(21\right)$

其中

为自适应无偏差滤波的渐消因子，

为自适应无偏差滤波的标量因子，

和

是根据标准卡尔曼滤波计算得到的理论值；最优增益阵的估计值

为

${\stackrel{\‾}{K}}_k^x={\stackrel{\‾}{P}}_k^x(-)H_k^T{\stackrel{\‾}{C}}_k^{-1}=\left[{\mathrm{\λ}}_k^x{P}_k^x(-)\right]H_k^T\left[\frac{1}{{\mathrm{\α}}_k^x}{C}_k^{-1}\right]=\frac{{\mathrm{\λ}}_k^x}{{\mathrm{\α}}_k^x}\left[P_k^x(-)H_k^T{C}_k^{-1}\right]=\frac{{\mathrm{\λ}}_k^x}{{\mathrm{\α}}_k^x}{K}_k^x---\left(22\right)$

其中自适应无偏差滤波的标量因子

为

${\mathrm{\α}}_k^x=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^x\right)}{\mathrm{trace}\left(C_k^x\right)}\}---\left(23\right)$

其中trace(·)表示矩阵的迹运算。

新息序列协方差的估计值

表示为

${\stackrel{\‾}{C}}_k^x={\mathrm{\α}}_k^x(H_k{P}_k^x(-)H_k^T+R_k)={\mathrm{\λ}}_k^x{H}_k{P}_k^x(-)H_k^T+R_k---\left(24\right)$

由公式(24)得到

${\mathrm{\α}}_k^x{H}_k{P}_k^x(-)H_k^T={\mathrm{\λ}}_k^x{H}_k{P}_k^x(-)H_k^T+(1-{\mathrm{\α}}_k^x)R_k---\left(25\right)$

则自适应无偏差滤波的渐消因子

为

${\mathrm{\λ}}_k^x=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^x\right)}{\mathrm{trace}\left(C_k^x\right)}\}---\left(26\right)$

步骤七：计算自适应无偏差滤波的状态估计值x_k(+)：

$x_k(+)=x_k(-)+K_k^x{\mathrm{\η}}_k^x---\left(27\right)$

步骤八：计算自适应偏差滤波的状态一步预测值b_k(-)：

                            b_k(-)＝A_k-1b_k-1(+)           (28)

其中，A_k-1为随机偏差系数矩阵，b_k-1(+)为自适应偏差滤波的状态估计值。

步骤九：计算自适应偏差滤波的预测均方误差矩阵

$P_k^b(-)={\mathrm{\λ}}_k^b(A_{k-1}{P}_{k-1}^b(+)A_{k-1}^T+Q_{k-1}^b)---\left(29\right)$

其中，

为自适应偏差滤波的估计均方误差矩阵，

为偏差系统噪声协方差，为自适应无偏差滤波的渐消因子。

步骤十：计算自适应偏差滤波的滤波增益矩阵

$K_k^b=P_k^b(-)N_k^T{[H_k{P}_k^x(-)H_x^T+R_k+N_k{P}_k^b(-)N_K^T]}^{-1}---\left(30\right)$

其中，R_k为观测噪声协方差，

为自适应无偏差滤波的预测均方误差矩阵。

步骤十一：计算自适应偏差滤波的估计均方误差矩阵

$P_k^b(+)=[I-K_k^b{N}_k]P_k^b(-)---\left(31\right)$

步骤十二：计算自适应偏差滤波的渐消因子

自适应偏差滤波的渐消因子

的计算方法和自适应无偏差滤波的渐消因子的计算方法相同，具体步骤为：

(1)计算k时刻观测量z_k的新息序列

${\mathrm{\η}}_k^b=z_k-H_k{x}_k(-)-N_k{b}_k(-)---\left(32\right)$

(2)计算k时刻新息序列协方差

的理论值：

$C_k^b=H_k{P}_k^x(-)H_k^T+R_k+N_k{P}_k^b(-)N_k^T---\left(33\right)$

(3)计算k时刻新息序列协方差的估计值

${\stackrel{\‾}{C}}_k^b={\mathrm{\λ}}_k^b{C}_k^b=\left\{\begin{array}{cc}\frac{{\mathrm{\λ}}_{k-1}{\mathrm{\η}}_k^b{\left({\mathrm{\η}}_k^b\right)}^T}{1+{\mathrm{\λ}}_{k-1}},& k>1\\ \frac{1}{2}{\mathrm{\η}}_0^b{\left({\mathrm{\η}}_0^b\right)}^T,& k=1\end{array}\right.---\left(34\right)$

其中，

为k＝0时的新息序列，

为k时刻的新息序列，为k-1时刻的渐消因子。

(4)计算自适应偏差滤波的渐消因子

${\mathrm{\λ}}_k^b=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^b\right)}{\mathrm{trace}\left(C_k^{\text{b}}\right)}\}---\left(35\right)$

步骤十三：计算自适应偏差滤波的状态估计值b_k(+)：

$b_k(+)=b_k(-)+K_k^b{\mathrm{\η}}_k^b---\left(36\right)$ 步骤十四：计算自适应二阶卡尔曼滤波的状态一步预测值

状态估计值

预测均方误差矩阵

和估计均方误差矩阵

分别为：

其中x_k(-)表示自适应无偏差滤波的状态一步预测值；x_k(+)表示自适应无偏差滤波的状态估计值；b_k(-)表示自适应偏差滤波的状态一步预测值；b_k(+)表示自适应偏差滤波的状态估计值；

自适应偏差滤波的预测均方误差矩阵；

表示自适应无偏差滤波的估计均方误差矩阵；

表示自适应偏差滤波的预测均方误差矩阵；

表示自适应偏差滤波的估计均方误差矩阵；U_k和V_k为步骤一中定义的混合矩阵。

本发明的优点在于：

(1)本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法，在系统模型不完整或其先验信息不准确时，通过渐消因子自适应的调节预测误差协方差矩阵，从而达到实时调整增益矩阵的目的，使滤波器接近最优，可以有效的抑制滤波器的发散；

(2)本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法，自适应渐消因子的估算方法计算简单，计算量小，适应性强，提高了滤波算法的可靠性；

(3)本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路及其滤波方法，基于自适应二阶卡尔曼滤波器提出的载波跟踪环路，不仅在高动态该环境下能提高跟踪环路的跟踪精度，而且在系统先验信息不准确时能够保持系统的稳定性，避免了跟踪环路的失锁。

附图说明

图1：本发明提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路的结构示意图；

图2：本发明中输入的GPS高动态信号的多普勒频率变化曲线；

图3：应用本发明提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环对多普勒频率的跟踪曲线图；

图4：本发明提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环对多普勒频率误差的跟踪曲线图。

图中：1-本地载波发生器；  2-积分清除器；      3-载波环鉴相器；

      4-环路滤波器；      5-自适应二阶卡尔曼滤波器。

具体实施方式

下面将结合附图对本发明作进一步的详细说明。

本发明提出一种基于自适应二阶卡尔曼滤波器的载波跟踪环路，包括本地载波发生器1、积分清除器2、载波环鉴相器3、环路滤波器4和自适应二阶卡尔曼滤波器5。本地载波发生器1的功能是复现一个本地载波信号；积分清除器2的功能相当于低通滤波器，消除同相支路i_p(n)与正交支路q_p(n)中的高频信号成分和噪声；载波环鉴相器3的功能是计算接收载波与复制载波之间的相位差异φ_e(n)；自适应二阶卡尔曼滤波器5的功能是使自适应二阶卡尔曼滤波器5递推估计后的载波相位误差经过环路滤波器4，以提高环路稳定性和跟踪精度；载波环路滤波器4的功能是降低噪声以便在其输出端对输入数字中频信号产生精确的估计。

射频前端输出的数字中频信号s_IF(n)分别与本地载波发生器1复现的载波及90°相移后的载波相乘后，再分别通过积分清除器2消除信号的高频分量和噪声，提高载噪比，然后分别将每个积分清除器2的相干积分结果I_p(n)和Q_p(n)均输出至载波环鉴相器3，利用积分清除器2输出的相干积分结果I_p(n)和Q_p(n)来估算当前跟踪环路的载波相位差异φ_e(n)，再将载波环鉴相器3的输出载波相位差异φ_e(n)作为自适二阶卡尔曼滤波器的观测值，经附带偏差的载波相位动态系统模型的自适应二阶卡尔曼滤波器5递推出相位差异的估计值进一步消除噪声的影响，提高载噪比。由于载体的动态性较高，环路滤波器4采用二阶环路滤波器，环路带宽可设为7Hz～10Hz，相位差异的估计值输出至环路滤波器4，经环路滤波器4输出的控制信号使本地载波发生器1实时调整，复现的载波与输入的数字中频信号保持一致。

所述的附带偏差的载波相位动态系统模型的状态方程具体为

$\left[\begin{array}{c}{\mathrm{\θ}}_{e,k}\\ f_{d,k}^{\mathrm{NCO}}\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{\Δt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_{e,k-1}\\ f_{d,k-1}^{\mathrm{NCO}}\end{array}\right]+\left[\begin{array}{cc}\mathrm{\Δt}& \frac{{\mathrm{\Δt}}^2}{2}\\ 0& 0\end{array}\right]\left[\begin{array}{c}{f}_{d,k-1}\\ f_{a,k-1}\end{array}\right]+\left[\begin{array}{cc}{W}_{\mathrm{\θ}}& 0\\ 0& 0\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{f}_{d,k}\\ f_{a,k}\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\Δt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{f}_{d,k-1}\\ f_{a,k-1}\end{array}\right]+\left[\begin{array}{cc}{W}_d& 0\\ 0& W_a\end{array}\right]---\left(2\right)$

附带偏差的载波相位动态系统模型的观测方程为

${\mathrm{\θ}}_{e,k-1}^{\mathrm{mea}}=\left[\begin{array}{cc}1& -\frac{\mathrm{\Δt}}{2}\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_{e,k-1}\\ f_{d,k-1}^{\mathrm{NCO}}\end{array}\right]+\left[\begin{array}{cc}\frac{\mathrm{\Δt}}{2}& \frac{{\mathrm{\Δt}}^2}{6}\end{array}\right]\left[\begin{array}{c}{f}_{d,k-1}\\ f_{a,k-1}\end{array}\right]+V_{k-1}---\left(3\right)$

其中，θ_e为本地载波发生器1的输出与输入信号的载波相位误差，f_d为输入信号的多普勒频移，f_a是由GPS卫星与接收机沿着视线速度方向的加速度引起的频率移动变化率，

为本地载波发生器NCO复制的多普勒频率，Δt为更新周期，等于预检测积分的时间。

表示估计的载波相位误差。θ_e，k为t_k时刻本地载波发生器1的输出与输入信号的载波相位差，

为t_k时刻本地载波发生器1复制的多普勒频率，

为t_k-1时刻本地载波发生器1复制的多普勒频率，f_d，k-1为t_k-1时刻输入信号的多普勒频移，f_a，k-1为t_k-1时刻由GPS卫星与接收机沿着视线速度方向的加速度引起的频率移动变化率，f_d，k为t_k时刻输入信号的多普勒频移，f_a，k为t_k时刻由GPS卫星与接收机沿着视线速度方向的加速度引起的频率移动变化率，W_n＝[W_θ，W_d，W_a]^T为噪声向量，由离散的高斯白噪声序列生成。W_θ，W_a，W_a为离散的噪声协方差分别是由接收机时钟引起的相位偏差，由时钟频率漂移引起的相位偏差，由卫星与接收机之间的加速度引起的相位偏差。

表为t_k-1时刻估计的载波相位误差，θ_e，k-1为t_k-1时刻本地载波发生器NCO的输出与输入信号的载波相位差，V_k-1为观测噪声矢量。

当本发明中输入中频信号为GPS高动态信号的多普勒频率变化曲线时，曲线如图2所示的，应用本发明提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路，能够准确跟踪图2中输入中频信号多普勒频率的变化，如图3所示。并且多普勒频率的估计误差很小，如图4所示，进一步说明了本发明提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路的稳定性和可靠性。

本发明还提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路的滤波方法，该滤波方法基于自适应二阶卡尔曼滤波方法，包括自适应无偏差滤波和自适应偏差滤波两部分。采用渐消因子抑制滤波器的记忆长度，充分利用现时的观测数据，减小陈旧量测值的影响，可以有效抑制滤波器的发散。在本发明的滤波方法中，利用新息序列协方差计算渐消因子，计算量小，适用于高动态环境下载波的快速跟踪。

本发明提出的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路的滤波方法，具体包括以下几个步骤：

步骤一：定义自适应二阶卡尔曼滤波的初始状态以及计算过程中的中间变量：

定义初始状态为：

$V_0=P_0^{\mathrm{xb}}{\left(P_0^b\right)}^{-1},$ $x_0(+)=x_0^{*}-V_0{b}_0^{*}---\left(4\right)$

$b_0(+)=b_0^{*},$ $P_0^b(+)=P_0^b---\left(5\right)$

$P_0^x(+)=P_0^x-V_0{P}_0^b{V}_0^T---\left(6\right)$

其中

和分别表示自适应二阶卡尔曼滤波初始状态向量x₀的均值和协方差，

和

分别表示自适应二阶卡尔曼滤波初始偏差向量b₀的均值和协方差，

表示x₀和b₀的互协方差矩阵。V₀，为定义的混合矩阵，无实际的物理含义，属于设定的中间变量，(·)^T表示矩阵的转置，(·)^-1表示求逆运算。x₀(+)、b₀(+)、

分别表示自适应无偏差滤波的初始状态估计值、自适应偏差滤波的初始状态估计值、自适应偏差滤波的初始估计均方误差矩阵、自适应无偏差滤波的初始估计均方误差矩阵。

定义中间变量：

N_k＝H_kU_k+D_k， $V_k=U_k-{K_k^{x}N}_k---\left(7\right)$

$U_k={\stackrel{\‾}{U}}_k[I-{\mathrm{\λ}}_k^b{Q}_{k-1}^b{\left[P_k^b(-)\right]}^{-1}]---\left(8\right)$

${\stackrel{\‾}{U}}_k=({\mathrm{\Φ}}_{k-1}{V}_{k-1}+B_{k-1})A_{k-1}^{-1}---\left(9\right)$

$u_k=({\stackrel{\‾}{U}}_{k+1}-U_{k+1})A_k{b}_k(+)---\left(10\right)$

${\stackrel{\‾}{Q}}_k^x=Q_k^x+U_{k+1}{Q}_k^b{\stackrel{\‾}{U}}_{k+1}---\left(11\right)$

其中H_k为观测矩阵，

为自适应偏差滤波的滤波增益矩阵，I为单位矩阵，

为自适应偏差滤波的渐消因子，

表示系统噪声协方差，

和

表示偏差系统噪声方差阵，

为自适应偏差滤波的预测均方误差矩阵，Φ_k-1为状态转移矩阵，A_k-1，B_k-1，D_k为随机偏差系数矩阵，

为系统噪声方差阵，u_k，U_k，

V_k，N_k，

为定义的混合矩阵，无实际的物理含义，属于中间变量。

步骤二：计算自适应无偏差滤波过程中的状态一步预测值x_k(-)：

                         x_k(-)＝Φ_k-1x_k-1(+)+u_k-1                (12)

其中，Φ_k-1为状态转移矩阵，x_k-1(+)为状态估计值，u_k-1为步骤一中定义的混合矩阵。

步骤三：计算自适应无偏差滤波方法中的预测均方误差矩阵

$P_k^x(-)={\mathrm{\λ}}_k^x({\mathrm{\Φ}}_{k-1}{P}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}^x)---\left(13\right)$

其中，

为估计均方误差矩阵，为系统噪声协方差，

为自适应无偏差滤波方法中的渐消因子。

步骤四：计算自适应无偏差滤波方法中的滤波增益矩阵

$K_k^x=P_k^x(-)H_k^T{[H_k{P}_k^x(-)H_K^T+R_k]}^{-1}---\left(14\right)$

其中，R_k为观测噪声协方差。

步骤五：计算自适应无偏差滤波方法中的估计均方误差矩阵

$P_k^x(+)=(I-K_k^x{H}_k)P_k^x(-)---\left(15\right)$

步骤六：计算自适应无偏差滤波方法中的渐消因子

(1)计算k时刻观测量z_k的新息序列

在自适应无偏差滤波中，t_k时刻观测向量Z_k的新息序列为

${\mathrm{\η}}_k^x=Z_k-H_k{x}_k(-)---\left(16\right)$

其中，H_k为观测矩阵，x_k(-)为滤波的状态一步预测值。

(2)计算k时刻新息序列协方差

的理论值：

所述的自适应无偏差滤波方法的计算过程中忽略了偏差量b_k，因此等效于自适应渐消卡尔曼滤波方法。

自适应渐消卡尔曼滤波方法与标准卡尔曼滤波方法不同之处仅在计算预测均方误差协方差

时增加一个渐消因子

tk时刻预测均方误差协方差为

$P_k^x(-)={\mathrm{\λ}}_k^x({\mathrm{\Φ}}_{k-1}{P}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}^x)---\left(17\right)$

其中，Φ_k-1为状态转移矩阵，为估计均方误差矩阵，(·)^T表示矩阵的转置，

为系统噪声的协方差，t_k为时间量。推导新息序列协方差

的理论值为

$C_k^x=E\left[{\mathrm{\η}}_k^x{\left({\mathrm{\η}}_k^x\right)}^T\right]=H_k{P}_k^x(-)H_k^T+R_k---\left(18\right)$

其中，E[·]表示求解协方差矩阵，R_k为量测噪声方差阵。

(3)计算k时刻新息序列协方差的估计值

对线性最优卡尔曼滤波，当滤波增益矩阵

为最优增益矩阵时，新息序列

是白噪声序列。则新息序列的自相关函数等于零，即

$E\left[{\mathrm{\η}}_{k+j}^x{\left({\mathrm{\η}}_k^x\right)}^T\right]=P_k^x(-)H_k^T-K_k^x{C}_k^x=0---\left(19\right)$

当增益阵

为最优增益阵时，新息序列不相关，即新息序列处处保持正交。

当载波相位动态系统模型不完整或者先验信息不准确时，新息序列协方差的实际值与计算出的理论值

不同。因此，新息序列的自相关函数就不一定等于零。基于以上情况，可以实时的调整增益阵使新息序列保持相互正交，即使式(19)等于零成立，此时公式(19)中的新息序列协方差

根据新息序列

估计得出(此时的

就是新息序列的估计值

)，而不是由式(18)计算出。

计算新息序列协方差的估计值

${\stackrel{\‾}{C}}_k^x=\left\{\begin{array}{cc}\frac{{\mathrm{\λ}}_{k-1}^x{\mathrm{\η}}_k^x{\left({\mathrm{\η}}_k^x\right)}^T}{1+{\mathrm{\λ}}_{k-1}^x},& k>1\\ \frac{1}{2}{\mathrm{\η}}_0^x{\left({\mathrm{\η}}_0^x\right)}^T,& k=1\end{array}\right.---\left(20\right)$

其中，

为k＝0时的新息序列，为k时刻的新息序列，

为k-1时刻的渐消因子。

(4)计算渐消因子

为得到增益阵调整之后的增益阵

设预测均方误差协方差的估计值和新息序列协方差的估计值

分别为

${\stackrel{\‾}{P}}_k^x(-)={\mathrm{\λ}}_k^x{P}_k^x(-),$ ${\stackrel{\‾}{C}}_k^x={\mathrm{\α}}_k^x{C}_k^x---\left(21\right)$

其中

为渐消因子，

为标量因子，

和

是根据标准卡尔曼滤波计算得到的理论值，而和是由公式(5)计算得到的估计值。由公式(5)，得到增益阵的估计值为

${\stackrel{\‾}{K}}_k^x={\stackrel{\‾}{P}}_k^x(-)H_k^T{\stackrel{\‾}{C}}_k^{-1}=\left[{\mathrm{\λ}}_k^x{P}_k^x(-)\right]H_k^T\left[\frac{1}{{\mathrm{\α}}_k^x}{C}_k^{-1}\right]=\frac{{\mathrm{\λ}}_k^x}{{\mathrm{\α}}_k^x}\left[P_k^x(-)H_k^T{C}_k^{-1}\right]=\frac{{\mathrm{\λ}}_k^x}{{\mathrm{\α}}_k^x}{K}_k^x---\left(22\right)$

其中标量因子

被估算为

${\mathrm{\α}}_k^x=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^x\right)}{\mathrm{trace}\left(C_k^x\right)}\}---\left(23\right)$

其中trace(·)表示矩阵的迹运算。

当前新息序列协方差的估计值

表示为

${\stackrel{\‾}{C}}_k^x={\mathrm{\α}}_k^x(H_k{P}_k^x(-)H_k^T+R_k)={\mathrm{\λ}}_k^x{H}_k{P}_k^x(-)H_k^T+R_k---\left(24\right)$

新息序列协方差的估计值

反应了当前滤波误差的作用，当载波相位动态模型不完整时，由于不完整信息的作用新息序列协方差和预测均方误差

的值会增加。从公式(24)可知，通过标量因子

使新息序列协方差增大是通过渐消因子

使预测均方误差协方差

增大得到的。因此，载波相位动态模型中不完整信息的作用可以通过增加预测均方误差矩阵

的值来补偿。

由公式(24)可得

${\mathrm{\α}}_k^x{H}_k{P}_k^x(-)H_k^T={\mathrm{\λ}}_k^x{H}_k{P}_k^x(-)H_k^T+(1-{\mathrm{\α}}_k^x)R_k---\left(25\right)$

由于则

当增加的预测均方误差协方差

较小时，由较小，可以把

忽略；当增加的预测均方误差协方差

大于量测噪声方差阵R_k时，认为量测噪声方差阵R_k较小，可以把忽略。因此

和

近似相等，假设

则

因此自适应无偏差滤波方法中的渐消因子

计算方法为

${\mathrm{\λ}}_k^x=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^x\right)}{\mathrm{trace}\left(C_k^x\right)}\}---\left(26\right)$

步骤七：计算自适应无偏差滤波方法中的状态估计值x_k(+)：

$x_k(+)=x_k(-)+K_k^x{\mathrm{\η}}_k^x---\left(27\right)$

步骤八：计算自适应偏差滤波方法中的状态一步预测值b_k(-)：

                             b_k(-)＝A_k-1b_k-1(+)             (28)

其中，A_k-1为随机偏差系数矩阵，b_k-1(+)为自适应偏差滤波器的状态估计值。

步骤九：计算自适应偏差滤波方法中的预测均方误差矩阵

$P_k^b(-)={\mathrm{\λ}}_k^b(A_{k-1}{P}_{k-1}^b(+)A_{k-1}^T+Q_{k-1}^b)---\left(29\right)$

其中，为估计均方误差矩阵，

为系统噪声协方差，

为自适应无偏差滤波方法中的渐消因子。

步骤十：计算自适应偏差滤波方法中的滤波增益矩阵

$K_k^b=P_k^b(-)N_k^T{[H_k{P}_k^x(-)H_x^T+R_k+N_k{P}_k^b(-)N_K^T]}^{-1}---\left(30\right)$

其中，R_k为观测噪声协方差，为自适应无偏差滤波方法中的预测均方误差矩阵。

步骤十一：计算自适应偏差滤波方法中的估计均方误差矩阵

$P_k^b(+)=[I-K_k^b{N}_k]P_k^b(-)---\left(31\right)$

步骤十二：计算自适应偏差滤波方法中的渐消因子

自适应偏差滤波器中渐消因子

的计算方法和自适应无偏差滤波中渐消因子的的计算方法一样，具体计算步骤为：

(1)计算k时刻观测量z_k的新息序列

${\mathrm{\η}}_k^b=z_k-H_k{x}_k(-)-N_k{b}_k(-)---\left(32\right)$

(2)计算k时刻新息序列协方差

的理论值：

$C_k^b=H_k{P}_k^x(-)H_k^T+R_k+N_k{P}_k^b(-)N_k^T---\left(33\right)$

(3)计算k时刻新息序列协方差的估计值

${\stackrel{\‾}{C}}_k^b={\mathrm{\λ}}_k^b{C}_k^b=\left\{\begin{array}{cc}\frac{{\mathrm{\λ}}_{k-1}{\mathrm{\η}}_k^b{\left({\mathrm{\η}}_k^b\right)}^T}{1+{\mathrm{\λ}}_{k-1}},& k>1\\ \frac{1}{2}{\mathrm{\η}}_0^b{\left({\mathrm{\η}}_0^b\right)}^T,& k=1\end{array}\right.---\left(34\right)$

其中，

为k＝0时的新息序列，

为k时刻的新息序列，为k-1时刻的渐消因子。

(4)计算渐消因子

${\mathrm{\λ}}_k^b=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^b\right)}{\mathrm{trace}\left(C_k^{\text{b}}\right)}\}---\left(35\right)$

步骤十三：计算自适应偏差滤波方法中的状态估计值b_k(+)：

$b_k(+)=b_k(-)+K_k^b{\mathrm{\η}}_k^b---\left(36\right)$

步骤十四：计算自适应二阶卡尔曼滤波方法的状态一步预测值、状态估计值、预测均方误差矩阵和估计均方误差矩阵：

自适应二阶卡尔曼滤波方法是自适应无偏差滤波器和自适应偏差滤波器的输出结果的联合，计算得到自适应二阶卡尔曼滤波方法状态一步预测值

状态估计值

预测均方误差矩阵

和估计均方误差矩阵为：

Claims (4)

1.一种基于自适应二阶卡尔曼滤波器的载波跟踪环路，其特征在于：包括本地载波发生器、积分清除器、载波环鉴相器、环路滤波器和自适应二阶卡尔曼滤波器；

数字中频信号分别与本地载波发生器复现的载波及90°相移后的载波相乘后，再分别通过积分清除器提高载噪比，然后分别将每个积分清除器的相干积分结果均输出至载波环鉴相器，利用积分清除器输出的相干积分结果估算当前跟踪环路的载波相位差异，再将载波环鉴相器的输出载波相位差异作为自适二阶卡尔曼滤波器的观测值，经附带偏差的载波相位动态系统模型的自适应二阶卡尔曼滤波器递推出相位差异的估计值，相位差异的估计值输出至环路滤波器，经环路滤波器输出的控制信号使本地载波发生器实时调整。

2.根据权利要求1所述的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路，其特征在于：所述的环路滤波器采用二阶环路滤波器，环路带宽为7Hz～10Hz。

3.根据权利要求1所述的一种基于自适应二阶卡尔曼滤波器的载波跟踪环路，其特征在于：所述的附带偏差的载波相位动态系统模型的状态方程具体为：

$\left[\begin{array}{c}{\mathrm{\θ}}_{e,k}\\ f_{d,k}^{\mathrm{NCO}}\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{\Δt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_{e,k-1}\\ f_{d,k-1}^{\mathrm{NCO}}\end{array}\right]+\left[\begin{array}{cc}\mathrm{\Δt}& \frac{{\mathrm{\Δt}}^2}{2}\\ 0& 0\end{array}\right]\left[\begin{array}{c}{f}_{d,k-1}\\ f_{a,k-1}\end{array}\right]+\left[\begin{array}{cc}{W}_{\mathrm{\θ}}& 0\\ 0& 0\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{f}_{d,k}\\ f_{a,k}\end{array}\right]=\left[\begin{array}{cc}1& \mathrm{\Δt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{f}_{d,k-1}\\ f_{a,k-1}\end{array}\right]+\left[\begin{array}{cc}{W}_d& 0\\ 0& W_a\end{array}\right]---\left(2\right)$

附带偏差的载波相位动态系统模型的观测方程具体为：

${\mathrm{\θ}}_{e,k-1}^{\mathrm{mea}}=\left[\begin{array}{cc}1& -\frac{\mathrm{\Δt}}{2}\end{array}\right]\left[\begin{array}{c}{\mathrm{\θ}}_{e,k-1}\\ f_{d,k-1}^{\mathrm{NCO}}\end{array}\right]+\left[\begin{array}{cc}\frac{\mathrm{\Δt}}{2}& \frac{{\mathrm{\Δt}}^2}{6}\end{array}\right]\left[\begin{array}{c}{f}_{d,k-1}\\ f_{a,k-1}\end{array}\right]+V_{k-1}---\left(3\right)$

其中，θ_e为本地载波发生器NCO的输出与输入信号的载波相位误差，f_d为输入信号的多普勒频移，f_a是由GPS卫星与接收机沿着视线速度方向的加速度引起的频率移动变化率，

为本地载波发生器复制的多普勒频率，Δt为更新周期，

表示估计的载波相位误差，θ_e，k为t_k时刻本地载波发生器的输出与输入信号的载波相位差，

为t_k时刻本地载波发生器复制的多普勒频率，

为t_k-1时刻本地载波发生器复制的多普勒频率，f_d，k-1为t_k-1时刻输入信号的多普勒频移，f_a，k-1为t_k-1时刻由GPS卫星与接收机沿着视线速度方向的加速度引起的频率移动变化率，f_d，k为t_k时刻输入信号的多普勒频移，f_a，k为t_k时刻由GPS卫星与接收机沿着视线速度方向的加速度引起的频率移动变化率，W_n＝[W_θ，W_d，W_a]^T为噪声向量，W_θ、W_a、W_a为离散的噪声协方差分别是由接收机时钟引起的相位偏差、由时钟频率漂移引起的相位偏差、由卫星与接收机之间的加速度引起的相位偏差，

表为t_k-1时刻估计的载波相位误差，θ_e，k-1为t_k-1时刻本地载波发生器NCO的输出与输入信号的载波相位差，V_k-1为观测噪声矢量。

4.一种基于自适应二阶卡尔曼滤波器的载波跟踪环路的滤波方法，其特征在于：具体包括以下几个步骤：

步骤一：定义自适应二阶卡尔曼滤波的初始状态以及计算过程中的中间变量：

定义初始状态为：

$V_0=P_0^{\mathrm{xb}}{\left(P_0^b\right)}^{-1},$ $x_0(+)=x_0^{*}-V_0{b}_0^{*}---\left(4\right)$

$b_0(+)=b_0^{*},$ $P_0^b(+)=P_0^b---\left(5\right)$

$P_0^x(+)=P_0^x-V_0{P}_0^b{V}_0^T---\left(6\right)$

其中

和分别表示自适应二阶卡尔曼滤波初始状态向量x₀的均值和协方差，

和

分别表示自适应二阶卡尔曼滤波初始偏差向量b₀的均值和协方差，表示x₀和b₀的互协方差矩阵，V₀，为定义的混合矩阵；x₀(+)、b₀(+)、

分别表示自适应无偏差滤波的初始状态估计值、自适应偏差滤波的初始状态估计值、自适应偏差滤波的初始估计均方误差矩阵、自适应无偏差滤波的初始估计均方误差矩阵；

定义中间变量：

N_k＝H_kU_k+D_k， $V_k=U_k-{K_k^{x}N}_k---\left(7\right)$

$U_k={\stackrel{\‾}{U}}_k[I-{\mathrm{\λ}}_k^b{Q}_{k-1}^b{\left[P_k^b(-)\right]}^{-1}]---\left(8\right)$

${\stackrel{\‾}{U}}_k=({\mathrm{\Φ}}_{k-1}{V}_{k-1}+B_{k-1})A_{k-1}^{-1}---\left(9\right)$

$u_k=({\stackrel{\‾}{U}}_{k+1}-U_{k+1})A_k{b}_k(+)---\left(10\right)$

${\stackrel{\‾}{Q}}_k^x=Q_k^x+U_{k+1}{Q}_k^b{\stackrel{\‾}{U}}_{k+1}---\left(11\right)$

其中H_k为观测矩阵，

为自适应偏差滤波的滤波增益矩阵，I为单位矩阵，为自适应偏差滤波的渐消因子，

表示系统噪声协方差，

和

表示偏差系统噪声协方差，为自适应偏差滤波的预测均方误差矩阵，Φ_k-1为状态转移矩阵，A_k-1，B_k-1，D_k为随机偏差系数矩阵，u_k，U_k，

V_k，N_k，

为定义的混合矩阵；b_k(+)表示自适应偏差滤波的状态估计值；

步骤二：计算自适应无偏差滤波过程中的状态一步预测值x_k(-)：

                        x_k(-)＝Φ_k-1x_k-1(+)+u_k-1                 (12)

其中，Φ_k-1为状态转移矩阵，x_k-1(+)为状态估计值，u_k-1为步骤一中定义的混合矩阵；

步骤三：计算自适应无偏差滤波方法中的预测均方误差矩阵

$P_k^x(-)={\mathrm{\λ}}_k^x({\mathrm{\Φ}}_{k-1}{P}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}^x)---\left(13\right)$

其中，

为估计均方误差矩阵，

为系统噪声协方差，为自适应无偏差滤波的渐消因子；

步骤四：计算自适应无偏差滤波的滤波增益矩阵

$K_k^x=P_k^x(-)H_k^T{[H_k{P}_k^x(-)H_K^T+R_k]}^{-1}---\left(14\right)$

其中，R_k为观测噪声协方差；

步骤五：计算自适应无偏差滤波的估计均方误差矩阵

$P_k^x(+)=(I-K_k^x{H}_k)P_k^x(-)---\left(15\right)$

步骤六：计算自适应无偏差滤波的渐消因子

(1)计算k时刻观测量z_k的新息序列

${\mathrm{\η}}_k^x=Z_k-H_k{x}_k(-)---\left(16\right)$

其中，H_k为观测矩阵，x_k(-)为自适应无偏差滤波的状态一步预测值；

(2)计算k时刻新息序列协方差

的理论值：

t_k时刻自适应无偏差滤波的预测均方误差协方差矩阵

为

$P_k^x(-)={\mathrm{\λ}}_k^x({\mathrm{\Φ}}_{k-1}{P}_{k-1}^x(+){\mathrm{\Φ}}_{k-1}^T+Q_{k-1}^x)---\left(17\right)$

其中Φ_k-1为状态转移矩阵，

自适应无偏差滤波的为估计均方误差矩阵，

为系统噪声的协方差，t_k为时间量，新息序列协方差

的理论值为：

$C_k^x=E\left[{\mathrm{\η}}_k^x{\left({\mathrm{\η}}_k^x\right)}^T\right]=H_k{P}_k^x(-)H_k^T+R_k---\left(18\right)$

其中E[·]表示求解协方差矩阵，R_k为量测噪声方差阵；

(3)计算k时刻新息序列协方差的估计值

当滤波增益矩阵

为最优增益矩阵时，新息序列

是白噪声序列，则新息序列的自相关函数等于零：

$E\left[{\mathrm{\η}}_{k+j}^x{\left({\mathrm{\η}}_k^x\right)}^T\right]=P_k^x(-)H_k^T-K_k^x{C}_k^x=0---\left(19\right)$

计算新息序列协方差的估计值

${\stackrel{\‾}{C}}_k^x=\left\{\begin{array}{cc}\frac{{\mathrm{\λ}}_{k-1}^x{\mathrm{\η}}_k^x{\left({\mathrm{\η}}_k^x\right)}^T}{1+{\mathrm{\λ}}_{k-1}^x},& k>1\\ \frac{1}{2}{\mathrm{\η}}_0^x{\left({\mathrm{\η}}_0^x\right)}^T,& k=1\end{array}\right.---\left(20\right)$

其中，

为k＝0时的新息序列，

为k时刻的新息序列，

为k-1时刻的渐消因子；

(4)计算自适应无偏差滤波的渐消因子

设预测均方误差协方差的估计值

和新息序列协方差的估计值

分别为：

${\stackrel{\‾}{P}}_k^x(-)={\mathrm{\λ}}_k^x{P}_k^x(-),$ ${\stackrel{\‾}{C}}_k^x={\mathrm{\α}}_k^x{C}_k^x---\left(21\right)$

其中

为自适应无偏差滤波的渐消因子，

为自适应无偏差滤波的标量因子，

和

是根据标准卡尔曼滤波计算得到的理论值；最优增益阵的估计值

为

${\stackrel{\‾}{K}}_k^x={\stackrel{\‾}{P}}_k^x(-)H_k^T{\stackrel{\‾}{C}}_k^{-1}=\left[{\mathrm{\λ}}_k^x{P}_k^x(-)\right]H_k^T\left[\frac{1}{{\mathrm{\α}}_k^x}{C}_k^{-1}\right]=\frac{{\mathrm{\λ}}_k^x}{{\mathrm{\α}}_k^x}\left[P_k^x(-)H_k^T{C}_k^{-1}\right]=\frac{{\mathrm{\λ}}_k^x}{{\mathrm{\α}}_k^x}{K}_k^x---\left(22\right)$

其中自适应无偏差滤波的标量因子

为

${\mathrm{\α}}_k^x=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^x\right)}{\mathrm{trace}\left(C_k^x\right)}\}---\left(23\right)$

其中trace(·)表示矩阵的迹运算；

新息序列协方差的估计值

表示为

${\stackrel{\‾}{C}}_k^x={\mathrm{\α}}_k^x(H_k{P}_k^x(-)H_k^T+R_k)={\mathrm{\λ}}_k^x{H}_k{P}_k^x(-)H_k^T+R_k---\left(24\right)$

由公式(24)得到

${\mathrm{\α}}_k^x{H}_k{P}_k^x(-)H_k^T={\mathrm{\λ}}_k^x{H}_k{P}_k^x(-)H_k^T+(1-{\mathrm{\α}}_k^x)R_k---\left(25\right)$

则自适应无偏差滤波的渐消因子

为

${\mathrm{\λ}}_k^x=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^x\right)}{\mathrm{trace}\left(C_k^x\right)}\}---\left(26\right)$

步骤七：计算自适应无偏差滤波的状态估计值x_k(+)：

$x_k(+)=x_k(-)+K_k^x{\mathrm{\η}}_k^x---\left(27\right)$

步骤八：计算自适应偏差滤波的状态一步预测值b_k(-)：

                            b_k(-)＝A_k-1b_k-1(+)               (28)

其中，A_k-1为随机偏差系数矩阵，b_k-1(+)为自适应偏差滤波的状态估计值；

步骤九：计算自适应偏差滤波的预测均方误差矩阵

$P_k^b(-)={\mathrm{\λ}}_k^b(A_{k-1}{P}_{k-1}^b(+)A_{k-1}^T+Q_{k-1}^b)---\left(29\right)$

其中，

为自适应偏差滤波的估计均方误差矩阵，

为偏差系统噪声协方差，

为自适应无偏差滤波的渐消因子；

步骤十：计算自适应偏差滤波的滤波增益矩阵

$K_k^b=P_k^b(-)N_k^T{[H_k{P}_k^x(-)H_x^T+R_k+N_k{P}_k^b(-)N_K^T]}^{-1}---\left(30\right)$

其中，R_k为观测噪声协方差，为自适应无偏差滤波的预测均方误差矩阵；

步骤十一：计算自适应偏差滤波的估计均方误差矩阵

$P_k^b(+)=[I-K_k^b{N}_k]P_k^b(-)---\left(31\right)$

步骤十二：计算自适应偏差滤波的渐消因子

自适应偏差滤波的渐消因子

的计算方法和自适应无偏差滤波的渐消因子的计算方法相同，具体步骤为：

(1)计算k时刻观测量z_k的新息序列

${\mathrm{\η}}_k^b=z_k-H_k{x}_k(-)-N_k{b}_k(-)---\left(32\right)$

(2)计算k时刻新息序列协方差

的理论值：

$C_k^b=H_k{P}_k^x(-)H_k^T+R_k+N_k{P}_k^b(-)N_k^T---\left(33\right)$

(3)计算k时刻新息序列协方差的估计值

${\stackrel{\‾}{C}}_k^b={\mathrm{\λ}}_k^b{C}_k^b=\left\{\begin{array}{cc}\frac{{\mathrm{\λ}}_{k-1}{\mathrm{\η}}_k^b{\left({\mathrm{\η}}_k^b\right)}^T}{1+{\mathrm{\λ}}_{k-1}},& k>1\\ \frac{1}{2}{\mathrm{\η}}_0^b{\left({\mathrm{\η}}_0^b\right)}^T,& k=1\end{array}\right.---\left(34\right)$

其中，

为k＝0时的新息序列，

为k时刻的新息序列，

为k-1时刻的渐消因子；

(4)计算自适应偏差滤波的渐消因子

${\mathrm{\λ}}_k^b=\max \{1,\frac{\mathrm{trace}\left({\stackrel{\‾}{C}}_k^b\right)}{\mathrm{trace}\left(C_k^{\text{b}}\right)}\}---\left(35\right)$

步骤十三：计算自适应偏差滤波的状态估计值b_k(+)：

$b_k(+)=b_k(-)+K_k^b{\mathrm{\η}}_k^b---\left(36\right)$ 步骤十四：计算自适应二阶卡尔曼滤波的状态一步预测值状态估计值

预测均方误差矩阵

和估计均方误差矩阵分别为：

其中x_k(-)表示自适应无偏差滤波的状态一步预测值；x_k(+)表示自适应无偏差滤波的状态估计值；b_k(-)表示自适应偏差滤波的状态一步预测值；b_k(+)表示自适应偏差滤波的状态估计值；

自适应偏差滤波的预测均方误差矩阵；

表示自适应无偏差滤波的估计均方误差矩阵；

表示自适应偏差滤波的预测均方误差矩阵；

表示自适应偏差滤波的估计均方误差矩阵；U_k和V_k为步骤一中定义的混合矩阵。

Images