CN102968552B - 一种卫星轨道数据预估与修正方法 - Google Patents

Abstract

本发明涉及卫星授时与卫星导航领域，具体的说是一种对卫星用户设备接收的轨道数据进行预估与修正的方法，首先用改进的切比雪夫预估法对轨道数据进行预估；根据最小二乘法原理，用已接收的轨道数据计算n阶切比雪夫多项式的系数；再利用得到的n阶切比雪夫多项式的系数通过切比雪夫拟合公式计算得到待预估时刻的轨道坐标估计值，用改进的模糊自适应卡尔曼滤波法对得到的轨道坐标估计值进行滤波；本发明提供了一种鲁棒性好、抗野值干扰能力强、预估误差小且连续性好、适合实时运算的卫星轨道数据预估与修正方法，以解决卫星授时和卫星导航系统实际应用中由于数据轨道不连续且预估不准确而造成的定时及定位错误问题。

Description

一种卫星轨道数据预估与修正方法

技术领域

本发明涉及卫星授时与卫星导航领域，具体的说是一种对卫星用户设备接收的轨道数据进行预估与修正的方法。

背景技术

通过对GPS卫星星历与北斗卫星星历的接收和解析，可以得到卫星轨道数据，就可以通过一系列计算得到卫星与用户之间的伪距，再进而通过一系列算法运算得到定时信息与定位信息，如果卫星轨道数据由于各种误差和干扰而不连续、不完整，就会影响到伪距的准确性从而影响到定时的精度与定位的精度。

目前，最小二乘递推算法与卡尔曼预测算法都是对卫星轨道数据进行预估常采用的算法，最小二乘递推算法的运算量大，受野值影响小，预测误差均值小，但预测误差连续性差；卡尔曼预测算法的运算量小，受野值影响大，预测误差均值大，但预测误差连续性好，这两种算法都有不足之处，尤其在卫星调轨期间轨道数据波动较大时，预估精度都不高。

发明内容

本发明为了克服上述常用算法的不足，用改进的切比雪夫预估算法做预估，再用改进的模糊自适应卡尔曼滤波算法对预估值做滤波与修正，提供了一种鲁棒性好、抗野值干扰能力强、预估误差小且连续性好、适合实时运算的卫星轨道数据预估与修正方法，以解决卫星授时和卫星导航系统实际应用中由于数据轨道不连续且预估不准确而造成的定时及定位错误问题。

本发明的技术方案是：

1.一种卫星轨道数据预估与修正方法，含有下列步骤：

步骤1.1：首先用改进的切比雪夫预估法对轨道数据进行预估；

根据最小二乘法原理，用已接收的轨道数据计算n阶切比雪夫多项式的系数；再利用得到的n阶切比雪夫多项式的系数通过切比雪夫拟合公式计算得到待预估时刻的轨道坐标估计值。

所述改进的切比雪夫预估法的具体步骤如下：

步骤1.1.1：根据公式将某一时间段的时间变量τ∈[t₁，t₂]归一化到区间τ∈[-1，1]上并且离散化；

步骤1.1.2：根据最小二乘法原理用待预估数据之前的十二个已收轨道数据计算n阶多项式系数，其中，前十二个已收轨道数据中若出现零值即剔除，只要确保参与系数求解的轨道数据不小于n个就能求得系数，其中n为正整数，且n≤5。

如果待预估时刻的预估坐标与实收坐标之差H大于自适应更新的门限BARa，则该时刻坐标不参与后一时刻最小二乘递推多项式系数的计算，其中BARa取待预估时刻之前三个H的升序中值做自适应实时更新；

如果用于计算某一个时刻最小二乘系数的坐标中最大值与最小值之差大于设定的门限值BARb，或者如果此时刻坐标与前一时刻坐标相同，则本次预估不重新计算多项式系数而使用前一时刻的系数；

用得到的n阶多项式系数代入切比雪夫多项式，通过切比雪夫拟合公式进行计算得出待预估时刻的轨道坐标估计值；

切比雪夫拟合公式为：

其中n为多项式阶数，Ci为多项式系数，变量Ti(τ)由如下递推公式确定：

$\{\begin{array}{c}{T}_0\left(\mathrm{\τ}\right)=1\\ T_1\left(\mathrm{\τ}\right)=\mathrm{\τ}\\ ......\\ T_n\left(\mathrm{\τ}\right)=2{\mathrm{\τT}}_{n-1}\left(\mathrm{\τ}\right)-T_{n-2}\left(\mathrm{\τ}\right)\end{array}.$

步骤1.2：用改进的模糊自适应卡尔曼滤波法对得到的轨道坐标估计值进行滤波；

首先根据构造的模糊函数D(k)自适应调整过程噪声协方差Q(k)与测量噪声协方差R(k)，然后根据卡尔曼滤波原理，将步骤1.1得到轨道坐标估计值作为观测量Z(k)，代入无控制的离散卡尔曼滤波基本公式进行计算，然后通过测量噪声协方差R(k)与过程噪声协方差Q(k)的计算公式得到的K时刻的最优估计值X(k/k)；最后通过修正函数φ(k)对最优估计值X(k/k)进行分段修正。

2.所述改进的模糊自适应卡尔曼滤波法的计算公式为：

1)X(k|k-1)＝A(k)*X(k-1|k-1)

2)P(k|k-1)＝A(k)*P(k-1|k-1)*A′(k)+Q(k-1)

3)X(k|k)＝X(k|k-1)+KG(k)*(Z(k)-H(k)*X(k|k-1))

4) $KG\left(k\right)=\frac{P\left(k\right|k-1)*H^{\′}\left(k\right)}{H\left(k\right)*P\left(k\right|k-1)*H^{\′}\left(k\right)+R\left(k\right)}$

5)P(k|k)＝P(k|k-1)-KG(k)*H(k)*P(k|k-1)

其中X(k-1/k-1)为k-1时刻坐标的最优估计值，X(k/k-1)为k时刻坐标的预测值，P(k/k-1)是k时刻预测误差协方差的预测值，P(k-1/k-1)是k-1时刻预测误差协方差的最优估计值，Q(k)是k时刻的测量噪声协方差，A(k)为k时刻的系统参数，Z(k)是k时刻的观测值，H(k)是k时刻的测量参数，KG(k)是k时刻的卡尔曼增益。

3.所述改进的模糊自适应卡尔曼滤波法的具体步骤为：

步骤3.1：构造模糊函数D(k)

$D\left(k\right)=(H\left(k\right)*P\left(k\right|k-1)*H^{\′}\left(k\right)+R\left(k\right))-\sqrt{\frac{\underset{k-2}{\overset{k}{\Σ}}\left(Z\right(k)-H\left(k\right)*X\left(k\right|k-1))}{3}}$

其中H(k)是k时刻的测量参数，P(k/k-1)是k时刻的预测误差协方差的预测值，Z(k)是k时刻的观测值，R(k)是k时刻的测量噪声协方差，X(k/k-1)是k时刻坐标的预测值。

测量噪声协方差R与过程噪声协方差Q的计算公式为：

$\left\{\begin{array}{cc}R\left(k\right)=R\left(k\right)-(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{R\left(k\right)}{\left|D\left(k\right)\right|})*R\left(k\right)& D\left(k\right)>{\mathrm{BAR}}_c>0\\ R\left(k\right)=R\left(k\right)-m*\left|D\left(k\right)\right|& {\mathrm{BAR}}_c>D\left(k\right)>-{\mathrm{BAR}}_c\\ R\left(k\right)=R\left(k\right)+(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{R\left(k\right)}{\left|D\left(k\right)\right|})*R\left(k\right)& D\left(k\right)<-{\mathrm{BAR}}_c<0\end{array}\right.$

$\left\{\begin{array}{cc}Q\left(k\right)=(Q\left(k\right)-(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{Q\left(k\right)}{\left|D\left(k\right)\right|})*Q\left(k\right))*I& D\left(k\right)>{\mathrm{BAR}}_c>0\\ Q\left(k\right)=Q\left(k\right)-m*\left|D\left(k\right)\right|& {\mathrm{BAR}}_c>D\left(k\right)>-{\mathrm{BAR}}_c\\ Q\left(k\right)=(Q\left(k\right)+(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{Q\left(k\right)}{\left|D\left(k\right)\right|})*Q\left(k\right))*I& D\left(k\right)<-{\mathrm{BAR}}_c<0\end{array}\right.$

其中R(k)是k时刻的测量噪声协方差，Q(k)是k时刻的过程噪声协方差，D(k)是k时刻的模糊函数，m为权系数，I为单位矩阵，BARc为针对构造的模糊函数D(k)所设定的门限。

其中选取初始最优坐标值X(0/0)＝[000]'，测量参数H＝[100]，系统参数A＝[110.5；011；001]；

然后利用改进的模糊自适应卡尔曼滤波法的计算公式(1)～(5)将待预估时刻的轨道坐标估计值代入观测值Z(k)进行迭代运算，得到k时刻坐标的最优估计值X(k/k)。

步骤3.2：构造修正函数

$\mathrm{\&phi;}\left(k\right)=\left\{\begin{array}{cc}1& \left|r\left(k\right)\right|\&le;{\mathrm{BAR}}_d\\ m*{\left(\frac{{\mathrm{BAR}}_d}{\left|r\left(k\right)\right|}\right)}^{\frac{1}{2}}& \left|r\left(k\right)\right|>{\mathrm{BAR}}_d\end{array}\right.$

其中BAR_d为针对构造的修正函数φ(k)所设定的门限，m为权系数，r(k)为新息，且r(k)＝Z(k)-H(k)*X(k|k-1)；当|r(k)|大于门限BAR_d时，使最优估计值X(k/k)乘以修正函数φ(k)，以减小超过门限BAR_d以外的新息给预估值带来的影响；而当|r(k)|小于门限BAR_d时，不做任何处理。

本发明的有益积极效果：

1.本发明的卫星轨道数据预估与修正方法，可以应用在GPS/北斗接收设备中，在计算时延及伪距之前，对解析出的卫星轨道数据进行预估与滤波修正。

2.本发明的卫星轨道数据预估与修正方法用改进的切比雪夫算法做预估，再用改进的模糊自适应卡尔曼滤波算法对预估值做滤波与修正，鲁棒性好、抗野值能力强，预估误差小且连续性好，适合实时运算的卫星轨道数据预估与修正方法，能够有效的解决卫星授时和卫星导航实际使用中由于轨道数据不连续且预估不准确而造成的定时及定位错误的问题。

附图说明

图1为本发明中改进的切比雪夫预估算法的流程示意图；

图2为本发明中改进的模糊自适应卡尔曼滤波算法的流程示意图；

图3为本发明对轨道实例数据处理的仿真示意图；

图4为本发明对轨道实例数据工程化处理的定时结果对比图。

具体实施方式

参见图1，图2所示：

1.一种卫星轨道数据预估与修正方法，含有下列步骤：

步骤1.1：首先用改进的切比雪夫预估法对轨道数据进行预估；

根据最小二乘法原理，用已接收的轨道数据计算n阶切比雪夫多项式的系数；再利用得到的n阶切比雪夫多项式的系数通过切比雪夫拟合公式计算得到待预估时刻的轨道坐标估计值；

所述改进的切比雪夫预估法的具体步骤如下：

步骤1.1.1：根据公式将某一时间段的时间变量τ∈[t₁，t₂]归一化到区间τ∈[-1，1]上并且离散化；

步骤1.1.2：根据最小二乘法原理用待预估数据之前的十二个已收轨道数据计算n阶多项式系数，其中，前十二个已收轨道数据中若出现零值即剔除，只要确保参与系数求解的轨道数据不小于n个就能求得系数，其中n为正整数，且n≤5。

如果待预估时刻的预估坐标与实收坐标之差H大于自适应更新的门限BARa，则该时刻坐标不参与后一时刻最小二乘递推多项式系数的计算，其中BARa取待预估时刻之前三个H的升序中值做自适应实时更新；

如果用于计算某一个时刻最小二乘系数的坐标中最大值与最小值之差大于设定的门限值BARb，或者如果此时刻坐标与前一时刻坐标相同，则本次预估不重新计算多项式系数而使用前一时刻的系数；

用得到的n阶多项式系数代入切比雪夫多项式，通过切比雪夫拟合公式进行计算得出待预估时刻的轨道坐标估计值；

切比雪夫拟合公式为：

其中n为多项式阶数，Ci为多项式系数，变量Ti(τ)由如下递推公式确定：

$\{\begin{array}{c}{T}_0\left(\mathrm{\&tau;}\right)=1\\ T_1\left(\mathrm{\&tau;}\right)=\mathrm{\&tau;}\\ ......\\ T_n\left(\mathrm{\&tau;}\right)=2{\mathrm{\&tau;T}}_{n-1}\left(\mathrm{\&tau;}\right)-T_{n-2}\left(\mathrm{\&tau;}\right)\end{array}.$

步骤1.2：用改进的模糊自适应卡尔曼滤波法对得到的轨道坐标估计值进行滤波；

首先根据构造的模糊函数D(k)自适应调整过程噪声协方差Q(k)与测量噪声协方差R(k)，然后根据卡尔曼滤波原理，将步骤1.1得到轨道坐标估计值作为观测量Z(k)，代入无控制的离散卡尔曼滤波基本公式进行计算，然后通过测量噪声协方差R(k)与过程噪声协方差Q(k)的计算公式得到的K时刻的最优估计值X(k/k)；最后通过修正函数φ(k)对最优估计值X(k/k)进行分段修正。

2.所述改进的模糊自适应卡尔曼滤波法的计算公式为：

1)X(k|k-1)＝A(k)*X(k-1|k-1)

2)P(k|k-1)＝A(k)*P(k-1|k-1)*A′(k)+Q(k-1)

3)X(k|k)＝X(k|k-1)+KG(k)*(Z(k)-H(k)*X(k|k-1))

4) $KG\left(k\right)=\frac{P\left(k\right|k-1)*H^{\&prime;}\left(k\right)}{H\left(k\right)*P\left(k\right|k-1)*H^{\&prime;}\left(k\right)+R\left(k\right)}$

5)P(k|k)＝P(k|k-1)-KG(k)*H(k)*P(k|k-1)

其中X(k-1/k-1)为k-1时刻坐标的最优估计值，X(k/k-1)为k时刻坐标的预测值，P(k/k-1)是k时刻预测误差协方差的预测值，P(k-1/k-1)是k-1时刻预测误差协方差的最优估计值，Q(k)是k时刻的测量噪声协方差，A(k)为k时刻的系统参数，Z(k)是k时刻的观测值，H(k)是k时刻的测量参数，KG(k)是k时刻的卡尔曼增益。

3.所述改进的模糊自适应卡尔曼滤波法的具体步骤为：

步骤3.1：构造模糊函数D(k)

$D\left(k\right)=(H\left(k\right)*P\left(k\right|k-1)*H^{\&prime;}\left(k\right)+R\left(k\right))-\sqrt{\frac{\underset{k-2}{\overset{k}{\mathrm{\&Sigma;}}}(Z\left(k\right)-H\left(k\right)*X\left(k\right|k-1))}{3}}$

其中H(k)是k时刻的测量参数，P(k/k-1)是k时刻的预测误差协方差的预测值，Z(k)是k时刻的观测值，R(k)是k时刻的测量噪声协方差，X(k/k-1)是k时刻坐标的预测值。

测量噪声协方差R与过程噪声协方差Q的计算公式为：

$\left\{\begin{array}{cc}R\left(k\right)=R\left(k\right)-(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{R\left(k\right)}{\left|D\left(k\right)\right|})*R\left(k\right)& D\left(k\right)>{\mathrm{BAR}}_c>0\\ R\left(k\right)=R\left(k\right)-m*\left|D\left(k\right)\right|& {\mathrm{BAR}}_c>D\left(k\right)>-{\mathrm{BAR}}_c\\ R\left(k\right)=R\left(k\right)+(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{R\left(k\right)}{\left|D\left(k\right)\right|})*R\left(k\right)& D\left(k\right)<-{\mathrm{BAR}}_c<0\end{array}\right.$

$\left\{\begin{array}{cc}Q\left(k\right)=(Q\left(k\right)-(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{Q\left(k\right)}{\left|D\left(k\right)\right|})*Q\left(k\right))*I& D\left(k\right)>{\mathrm{BAR}}_c>0\\ Q\left(k\right)=Q\left(k\right)-m*\left|D\left(k\right)\right|& {\mathrm{BAR}}_c>D\left(k\right)>-{\mathrm{BAR}}_c\\ Q\left(k\right)=(Q\left(k\right)+(1-\frac{1}{\sqrt{{\text{1+BAR}}_c}})*(1-\frac{Q\left(k\right)}{\left|D\left(k\right)\right|})*Q\left(k\right))*I& D\left(k\right)<-{\mathrm{BAR}}_c<0\end{array}\right.$

其中R(k)是k时刻的测量噪声协方差，Q(k)是k时刻的过程噪声协方差，D(k)是k时刻的模糊函数，m为权系数，I为单位矩阵，BARc为针对构造的模糊函数D(k)所设定的门限。

其中选取初始最优坐标值X(0/0)＝[000]'，测量参数H＝[100]，系统参数A＝[110.5；011；001]；

然后利用改进的模糊自适应卡尔曼滤波法的计算公式(1)～(5)将待预估时刻的轨道坐标估计值代入观测值Z(k)进行迭代运算，得到k时刻坐标的最优估计值X(k/k)。

步骤3.2：构造修正函数

$\mathrm{\&phi;}\left(k\right)=\left\{\begin{array}{cc}1& \left|r\left(k\right)\right|\&le;{\mathrm{BAR}}_d\\ m*{\left(\frac{{\mathrm{BAR}}_d}{\left|r\left(k\right)\right|}\right)}^{\frac{1}{2}}& \left|r\left(k\right)\right|>{\mathrm{BAR}}_d\end{array}\right.$

其中BARd为针对构造的修正函数φ(k)所设定的门限，m为权系数，r(k)为新息，且r(k)＝Z(k)-H(k)*X(k|k-1)；当|r(k)|大于门限BARd时，使最优估计值X(k/k)乘以修正函数φ(k)，以减小超过门限BARd以外的新息给预估值带来的影响；而当|r(k)|小于门限BARd时，不做任何处理。

本发明使用北斗一代卫星某日期某星某波真实的广播电文信息，在计算机上对轨道数据进行了本发明所述方法的模拟仿真，得到了每一步骤处理之后的卫星三维轨道，参见图3所示，图3中左边的部分表示的是用接收机接收到的未经滤波的原始轨道数据仿真的三维轨道，由图可见，由于数据接收误差太大，无法仿真出连续的轨道；图3中间的部分是原始轨道数据在经过本发明的方法步骤1.1的运算之后得到的数据仿真出的三维轨道，由图可见已经可以仿真出轨道，但是轨道并不平滑，有毛刺，说明此时的轨道数据还不连续与稳定；图3右边的部分是再经过本发明步骤1.2的运算之后得到的数据仿真出的三维轨道，与图3中间的部分相比，右边的部分中轨到更加连续与稳定，毛刺已被消除。

本发明同样使用北斗一代卫星某日期某波真实的广播电文信息在工程上进行了数据处理，得到了接收机输出的用于授时的1PPS，如图4所示，图4中的横坐标表示一天中的秒数，纵坐标表示接收机输出的1PPS与标准1PPS之差，正值表示“延后”，负值表示“提前”，单位为纳秒，如果纵坐标为0，表示输出1PPS与标准1PPS无误差。

不论“延后”还是“提前”，与标准1PPS的差越小，则定时性能越好。

由于原始轨道数据无法构建出真实轨道，亦无法算得到准确的1PPS，所以图4中没有表示出经原始轨道数据解算出的1PPS偏差。

图4中上面的曲线是经本发明步骤1.1之后的轨道数据运算得到的1PPS偏差，下面的曲线是再经本发明步骤1.2之后的轨道数据运算得到的1PPS偏差，卫星轨道坐标预估与修正越精确，计算得到的1PPS就与标准1PPS越接近。

Claims (3)

1.一种卫星轨道数据预估与修正方法，其特征在于：含有下列步骤：

步骤1.1：首先用改进的切比雪夫预估法对轨道数据进行预估；

根据最小二乘法原理，用已接收的轨道数据计算n阶切比雪夫多项式的系数；再利用得到的n阶切比雪夫多项式的系数通过切比雪夫拟合公式计算得到待预估时刻的轨道坐标估计值；所述改进的切比雪夫预估法的具体步骤如下：

步骤1.1.1：根据公式将某一时间段的时间变量t∈[t₁,t₂]归一化到区间t∈[-1,1]上并且离散化；

步骤1.1.2：根据最小二乘法原理用待预估数据时刻之前的十二个已收轨道数据计算n阶多项式系数，前十二个已收轨道数据中若出现零值即剔除，其中n为正整数，且n≤5，

如果待预估时刻的预估坐标与实收坐标之差H大于自适应更新的门限BARa，则该时刻坐标不参与后一时刻最小二乘递推多项式系数的计算，其中BARa取待预估时刻之前的三个H的升序中值做自适应实时更新；

如果用于计算某一个时刻最小二乘系数的坐标中最大值与最小值之差大于设定的门限值BARb，或者如果此时刻坐标与前一时刻坐标相同，则本次预估不重新计算多项式系数而使用前一时刻的系数；

用得到的n阶多项式系数代入切比雪夫多项式，通过所述的切比雪夫拟合公式进行计算得出待预估时刻的轨道坐标估计值；

其中切比雪夫拟合公式为：

其中n为多项式阶数，C_i为多项式系数，变量T_i(τ)由如下递推公式确定：

$\left\{\begin{array}{c}{T}_0\left(\mathrm{\&tau;}\right)=1\\ T_1\left(\mathrm{\&tau;}\right)=\mathrm{\&tau;}\\ \mathrm{........}\\ T_n\left(\mathrm{\&tau;}\right)=2\mathrm{\&tau;}{T}_{n-1}\left(\mathrm{\&tau;}\right)-T_{n-2}\left(\mathrm{\&tau;}\right)\end{array}\right.$

步骤1.2：用改进的模糊自适应卡尔曼滤波法对得到的轨道坐标估计值进行滤波；

首先根据构造的模糊函数D(k)自适应调整过程噪声协方差Q(k)与测量噪声协方差R(k)，然后根据卡尔曼滤波原理，将步骤1.1得到轨道坐标估计值作为观测量Z(k)，代入无控制的离散卡尔曼滤波基本公式进行计算，然后通过测量噪声协方差R(k)与过程噪声协方差Q(k)的计算公式得到的k时刻的最优估计值X(k/k)；最后通过修正函数φ(k)对最优估计值X(k/k)进行分段修正。

2.根据权利要求1所述的卫星轨道数据预估与修正方法，其特征在于：所述改进的模糊自适应卡尔曼滤波法的计算公式为：

(1)X(k|k-1)＝A(k)*X(k-1|k-1)

(2)P(k|k-1)＝A(k)*P(k-1|k-1)*A'(k)+Q(k-1)

(3)X(k|k)＝X(k|k-1)+KG(k)*(Z(k)-H(k)*X(k|k-1))

$\left(4\right)---KG\left(k\right)=\frac{P\left(k\right|k-1)*H^{\&prime;}\left(k\right)}{H\left(k\right)*P\left(k\right|k-1)*H^{\&prime;}\left(k\right)+R\left(k\right)}$

(5)P(k|k)＝P(k|k-1)-KG(k)*H(k)*P(k|k-1)

其中X(k-1/k-1)为k-1时刻坐标的最优估计值，X(k/k-1)为k时刻坐标的预测值，P(k/k-1)是k时刻预测误差协方差的预测值，P(k-1/k-1)是k-1时刻预测误差协方差的最优估计值，Q(k)是k时刻的测量噪声协方差，A(k)为k时刻的系统参数，Z(k)是k时刻的观测值，H(k)是k时刻的测量参数，KG(k)是k时刻的卡尔曼增益。

3.根据权利要求2所述的卫星轨道数据预估与修正方法，其特征在于：所述改进的模糊自适应卡尔曼滤波法的具体步骤为：

步骤3.1：构造模糊函数D(k)

$D\left(k\right)=\left(H\right(k)*P(k|k-1)*H^{\&prime;}(k)+R(k\left)\right)-\sqrt{\frac{\underset{k-2}{\overset{k}{\&Sigma;}}\left(Z\right(k)-H(k)*X(k|k-1\left)\right)}{3}}$

其中H(k)是k时刻的测量参数，P(k/k-1)是k时刻的预测误差协方差的预测值，Z(k)是k时刻的观测值，R(k)是k时刻的测量噪声协方差，X(k/k-1)是k时刻坐标的预测值；

测量噪声协方差R(k)与过程噪声协方差Q(k)的计算公式为：

$\left\{\begin{array}{cc}R\left(k\right)=R\left(k\right)-(1-\frac{1}{\sqrt{1+{\mathrm{BAR}}_c}})*(1-\frac{R\left(k\right)}{\left|D\left(k\right)\right|})*R\left(k\right)& D\left(k\right)>{\mathrm{BAR}}_c>0\\ R\left(k\right)=R\left(k\right)-m*\left|D\left(k\right)\right|& {\mathrm{BAR}}_c>D\left(k\right)>-{\mathrm{BAR}}_c\\ R\left(k\right)=R\left(k\right)+(1-\frac{1}{\sqrt{1+{\mathrm{BAR}}_c}})*(1-\frac{R\left(k\right)}{\left|D\left(k\right)\right|})*R\left(k\right)& D\left(k\right)<-{\mathrm{BAR}}_c<0\end{array}\right.$

$\left\{\begin{array}{cc}Q\left(k\right)=\left(Q\right(k)-(1-\frac{1}{\sqrt{1+{\mathrm{BAR}}_c}})*(1-\frac{Q\left(k\right)}{\left|D\left(k\right)\right|})*Q\left(k\right))*I& D\left(k\right)>{\mathrm{BAR}}_c>0\\ Q\left(k\right)=Q\left(k\right)-m*\left|D\left(k\right)\right|& {\mathrm{BAR}}_c>D\left(k\right)>-{\mathrm{BAR}}_c\\ Q\left(k\right)=\left(Q\right(k)+(1-\frac{1}{\sqrt{1+{\mathrm{BAR}}_c}})*(1-\frac{Q\left(k\right)}{\left|D\left(k\right)\right|})*Q\left(k\right))*I& D\left(k\right)<-{\mathrm{BAR}}_c<0\end{array}\right.$

其中R(k)是k时刻的测量噪声协方差，Q(k)是k时刻的过程噪声协方差，D(k)是k时刻的模糊函数，m为权系数，I为单位矩阵，BAR_c为针对构造的模糊函数D(k)所设定的门限；

其中选取初始最优坐标值X(0/0)＝[000]'，测量参数H＝[100]，系统参数A＝[110.5；011；001],然后利用所述的改进的模糊自适应卡尔曼滤波法的计算公式(1)～(5)将待预估时刻的轨道坐标估计值代入观测值Z(k)进行迭代运算，得到k时刻坐标的最优估计值X(k/k)；

步骤3.2：构造修正函数

$\mathrm{\&phi;}\left(k\right)=\left\{\begin{array}{cc}1& \left|r\left(k\right)\right|\&le;{\mathrm{BAR}}_d\\ m*{\left(\frac{{\mathrm{BAR}}_d}{\left|r\left(k\right)\right|}\right)}^{\frac{1}{2}}& \left|r\left(k\right)\right|>{\mathrm{BAR}}_d\end{array}\right.$

其中BAR_d为针对构造的修正函数φ(k)所设定的门限，m为权系数，r(k)为新息，且

r(k)＝Z(k)-H(k)*X(k|k-1)；当|r(k)|大于门限BAR_d时，使最优估计值X(k/k)乘以修正函数φ(k)，以减小超过门限BAR_d以外的新息给预估值带来的影响；而当|r(k)|小于门限BAR_d时，不做任何处理。