CN102608631B - 基于模糊逻辑的自适应强跟踪ukf定位滤波算法 - Google Patents

Abstract

本发明公开了一种基于模糊逻辑的自适应强跟踪UKF定位滤波算法，包括（1）建立定位滤波模型；（2）设定滤波器初始参数；（3）运用UKF滤波算法对状态量进行滤波；（4）运用模糊逻辑系统求解强跟踪自适应算法中的软化因子；（5）强跟踪自适应算法中自适应因子求解；（6）历元时刻递加1，读取下一时刻观测，返回步骤（4），直至结束。本发明通过在UKF滤波算法基础上引入强跟踪自适应算法，并在强跟踪自适应算法中采用新递推算法估计信息协方差矩阵，对强跟踪算法中软化因子采用模糊逻辑推理系统进行求解，根据历元时刻滤波器的工作状况对软化因子进行实时估计。在卫星导航用户接收机位置估计中，可大大地提高载体适应动态性的能力和定位性能。

Description

基于模糊逻辑的自适应强跟踪UKF定位滤波算法

技术领域

本发明涉及卫星导航接收机位置参数解算，尤其是涉及一种基于模糊逻辑的自适应强跟踪UKF定位滤波算法。

背景技术

卫星导航技术的飞速发展已逐渐取代了无线电导航、天文导航等传统导航技术，而成为一种普遍采用的导航定位技术，并在精度、实时性、全天候等方面取得了长足进步。目前，世界上多个国家和地区已经建立或者正在建设全球卫星定位导航系统，具体包括美国建立的GPS系统、俄罗斯建立的GLONASS系统、欧盟正在建设的“伽利略”系统和中国正在建设的“北斗二代”系统。导航系统中的定位解算模块是根据卫星信号的量测信息采用相应算法对用户接收机位置、速度和时钟参数进行解算，是卫星导航系统实现的关键技术之一。

在卫星导航接收机中，导航定位解算是用户接收机定位最为关键的一个环节，采用定位算法的优劣直接导致用户定位精度的好坏。传统接收机用户定位的算法选择为最小二乘，具体实现步骤：(1)获取跟踪环路输出伪距信息ρ＝[ρ₁，ρ₂Lρ_N]^T，其中N代表跟踪通道输出伪距数目；(2)用户接收机初始位置估计X₀＝[x₀，y₀，z₀]^T；3)量测方程线性化，即量测方程 ${\mathrm{\ρ}}_i=\sqrt{{(X_i-X)}^2+{(Y_i-Y)}^2+{(Z_i-Z)}^2}+{\mathrm{CVt}}_u$ 在初始用户位置估计X₀＝[x₀，y₀，z₀]^T处线性化得到[Δρ₁，Δρ₂LΔρ_m]^T＝H[Δx，Δy，Δz，Ct_u]^T；(4)由量测方程得到状态量的最小二乘估计 $\mathrm{\Δ}\hat{X}={\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\Δ\ρ};$ (5)更新用户接收机的位置即 ${[X,Y,Z]}_k^T={[X,Y,Z]}_{k-1}^T+{[\mathrm{\Δx},\mathrm{\Δy},\mathrm{\Δz}]}_{k|k-1}^T;$ (6)读取观测数据，转至步骤(3)继续执行，直至观测数据结束。

最小二乘算法应用于运动状态估计中，其算法简单，使用与对常值向量或随机向量的估计，并且在对被估计量和量测误差缺乏了解的情况下仍能使用，但是由于缺乏对系统动力学状态的考虑，估计精度并不很高，特别是对于非静止运动状态，存在较大的估计误差。因此，有些研究学者提出了卡尔曼滤波算法，用以加强系统动力学的估计，以提高状态量的估计精度。

用户接收机通常采用最小二乘算法(参见Fundamentals of GlobalPositioning System Receivers-A Software Approach，SENOND EDIRION，Canada，James B and Yen T，2005.)和卡尔曼Kalman滤波算法(参见Jwo D J andWang S H，Adaptive Fuzzy Strong Tracking Extended Kalman Filtering for GPSNavigation，IEEE SENSORS JOURNAL.VOL.7，NO.5，MAY 2007.和Liu J andLu M Q，An Adaptive UKF Filtering Algorithm for GPS Position Estimation，IEEEXPLORE Wireless Communications，Networking and Mobile Computing.2009.WiCom 09.5th International Conference on Issue Data：24-26Sept.2009.)进行用户接收机位置的解算。

最小二乘算法虽然算法简单，但由于缺乏对系统动力学状态的考虑，估计精度并不很高，特别是对于非静止运动状态，存在较大的估计误差。而Kalman滤波算法虽能够解决定位解算中的非线性问题，但是对于先验信息要求极为严格，同时定位精度受到载体动态性的限制。

发明内容

本发明为克服现有技术的不足，提出一种基于模糊逻辑的自适应强跟踪UKF定位滤波算法，该方法在传统UKF算法基础上，将自适应强跟踪算法和模糊逻辑系统结合，大大地提高了卫星导航系统定位算法的性能。

根据本发明的一个方面，提供一种基于模糊逻辑的自适应强跟踪UKF定位滤波算法，包括以下步骤：

(1)建立滤波模型；

(2)设定滤波器初始参数；

(3)运用UKF滤波算法对状态量进行滤波；

(4)运用模糊逻辑系统求解强跟踪自适应算法中的软化因子；

(5)强跟踪自适应算法中自适应因子求解；

(6)历元时刻递加1，读取下一时刻观测，返回步骤(4)，直至结束。

进一步地，步骤(1)中所述滤波模型包括：

针对低动态环境，根据接收机的位置、速度、时钟钟差和频差建立的常速CV模型；针对高动态环境，根据接收机的位置、速度、加速度、时钟钟差和频差建立的常加速度CA模型。

进一步地，步骤(2)中所述设定滤波器初始参数包括：

设置系统噪声矩阵Q(t)和观测噪声矩阵R；

设置状态量初始估计X₀和初始状态估计误差协方差P₀；

滤波时刻k清零，将自适应因子赋值1，即自适应因子λ_i，k＝1。

进一步地，步骤(3)中所述运用UKF滤波算法对状态量X进行滤波的步骤包括：

初始状态采样：初始状态量采样，得到初始状态sigma样本点及对应权重因子；

一步预测状态采样：根据所获得的初始状态sigma样本点，对一步预测状态和一步预测状态协方差矩阵进行估计；

一步预测状态采样二次采样：对获得的一步预测状态及一步预测状态协方差矩阵进行二次采样，得到二次采样的一步预测状态sigma样本点及对应权重因子；

输出一步预测：根据一步预测状态的二次采样输出观测量一步预测，之后进行观测量自协方差矩阵、观测量和状态量之间互协方差矩阵估计；

卡尔曼增益求解：通过获得的观测量自协方差矩阵和观测量与状态量的互协方差矩阵，求解卡尔曼增益矩阵，实现状态量估计更新和状态量估计协方差更新。

进一步地，步骤(4)中所述运用模糊逻辑系统求解强跟踪自适应算法中的软化因子的步骤包括：

获得滤波信息向量v_k；

模糊逻辑系统输入：通过滤波信息向量v_k，求解模糊逻辑系统输入r_i，其中模糊逻辑系统输入r_i为滤波信息向量v_k单步协方差矩阵的迹与单步理论信息方差阵的迹之比；

模糊逻辑系统求解：根据模糊逻辑系统输入r_i的变化规律，进行模糊逻辑系统的模糊化过程，设定模糊规则，进行模糊推理，去模糊化过程，实现软化因子输出。

所述模糊化过程中输入参量隶属度函数包括三角形函数，所述模型系统规则包括一阶T-S模型、IF-THEN形式。

进一步地，步骤(5)中所述强跟踪自适应算法中自适应因子λ_i，k求解的步骤包括：

由滤波信息向量v_k对滤波信息真实协方差V_k进行估计求解；

求解包含滤波信息协方差V_k和观测噪声估计的自适应矩阵N_k；

由自适应矩阵N_k和信息理论协方差矩阵M_k求解自适应因子中间参数C_i，k；

通过对中间参数C_i，k进行判定来对自适应因子λ_i，k赋值。

本发明与现有技术相比的优点在于：

(1)与传统的UKF算法相比，对一步预测状态进行了二次采样过程，减小了对于随机变量通过非线性系统后输出变量统计特性的估计误差，在一定程度上提高了状态量估计的精度。

(2)将强跟踪算法与UKF算法相结合，克服了传统UKF滤波算法容易受初始值和模型误差的缺点。

(3)强跟踪算法中的软化因子估计采用了模糊逻辑推理系统进行估计，用单次滤波信息协方差的迹与理论信息协方差的迹之比作为模糊逻辑系统的输入，时刻监测滤波器的工作状况，实时调整软化因子。

(4)自适应参数求解过程中，对于信息实际协方差矩阵估计时，采用了一种新的递推估计算法，更加直接的反应器载体实际的运动状态，同时也减小了计算量。

附图说明

图1是现行GPS定位最小二乘算法的流程图；

图2是现行UKF滤波算法的流程图；

图3是本发明实施例提供的自适应强跟踪UKF定位滤波算法的流程图；

图4是本发明实施例提供的强跟踪算法中软化因子的求解流程图；

图5是本发明实施例提供的强跟踪算法中自适应因子的求解流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明中处理的数据是卫星导航系统跟踪模块输出的测量伪距信息，同时还包括对应跟踪时刻卫星的位置和速度参数。用户接收机的定位是在地心地固ECEF坐标系下进行的，首先根据接收机通道卫星信号跟踪测量得到的伪距信息，对用户接收机的位置参数和时钟钟差参数进行解算。当通道跟踪有效卫星超过4颗，可用于接收机的实际定位解算。

下面结合附图，对本发明详细说明如下：

参见图3，该图示出了本发明实施例提供的自适应强跟踪UKF定位滤波算法的详细流程；

S1：建立滤波模型：

在建立滤波模型之前，本发明须要获取卫星测量伪距信息，以及对应跟踪时刻卫星在地心地固ECEF坐标系下的位置和速度参数；

首先，由卫星信号模拟器或者软件接收机处理实际数据得到卫星的测量伪距信息，一般情况下伪距信息获得频率为1Hz，同时得到对应跟踪时刻卫星在地心地固ECEF坐标系下的位置和速度参数。

当采用卫星信号模拟器生成卫星的测量伪距信息时(通常采用Renix文件还原伪距生成的方式)，须要首先确定使用历书的周数，设置仿真间隔和仿真总时间(其中可以包含相应选星算法，保证定位精度)。

由于卫星信号模拟器生成数据时，只模拟了时钟误差在生成伪距中，因此在生成的伪距中添加适量的白噪声，视为伪距中的环境噪声。

如果处理的数据是实际数据，经过软件接收机处理后，即可得到相应通道卫星的伪距信息和卫星的位置、速度参数信息。

然后，根据用户接收机(载体)的实际运动规律选取状态估计量和系统量测量，建立滤波模型；建立滤波模型包括建立以下模型：

建立CV模型：

对于载体运动的低动态环境，使用接收机的位置、速度和时钟参数作为状态量X＝[x，y，z，v_x，v_y，v_z，td，fd]^T _ECEF，其中x，y，z和v_x，v_y，v_z分别表示ECEF坐标系下三维的位置与速度分量；td，fd分别表示本地时钟的钟差和钟漂分量。

根据用户接收机的运动规律，建立CV模型，其中状态转移方程，如公式(1)所示。

其中，

X(t)＝[x(t)y(t)z(t)td]^T    (2)

$A_{4\×4}=\left[\begin{array}{cc}{0}_{3\×3}& 0\\ 0& -\mathrm{\κ}\end{array}\right]---\left(4\right)$

上式中分别表示载体运动在x，y，z方向上和时钟频率的；κ为载体晶振漂移系数，W(t)协方差阵 $Q\left(t\right)=\mathrm{diag}\{\mathrm{0,0,0,0},{\mathrm{\σ}}_x^2,{\mathrm{\σ}}_y^2,{\mathrm{\σ}}_z^2,{\mathrm{\σ}}_{\mathrm{td}}^2\},$ 和分别表示x，y，z速度方向和时钟上驱动噪声的强度。

建立CA模型：

对于载体运动的高动态环境，使用接收机的位置、速度、加速度和时钟参数作为状态量其中a_x，a_y，a_z表示三维加速度分量。

根据用户接收机的运动规律，建立CA模型。其中状态转移方程参照公式(1)建立，其中添加状态参量

根据选取的状态估计量，建立系统量测模型，其中用户接收机与第i颗卫星之间的测量伪距ρ_i如公式(5)所示。

${\mathrm{\ρ}}_i=\sqrt{{(X_i-X)}^2+{(Y_i-Y)}^2+{(Z_i-Z)}^2}+C\×\mathrm{td}+v_i---\left(5\right)$

其中，X_i，Y_i，Z_i和X，Y，Z分别表示第i颗卫星和用户接收机的三维位置参量，C表示电磁波在真空中的传播速度，v_i表示第i颗卫星测量伪距量测噪声。

S2：滤波器初始参数设定；

所设定的滤波器初始参数包括：系统噪声矩阵Q(t)和观测噪声矩阵R、状态量初始估计X₀和初始状态估计误差协方差P₀、滤波时刻k清零，将自适应因子赋值为1。

设置系统噪声矩阵Q(t)和观测噪声矩阵R：根据选取的状态估计量和系统量测量建立的状态模型和观测模型，对载体实际运动规律和观测噪声强度进行先验估计，设置系统噪声矩阵Q(t)和观测噪声矩阵R。

其中，系统噪声矩阵设置，如步骤(2)中推导，分别估计不同坐标轴方向上的驱动噪声强度σ²。观测噪声矩阵R的设置方法依据不同卫星跟踪环路相互独立，观测噪声彼此不相关，既而可得其观测噪声矩阵R＝diag{R₁，R₂，L，R_N}，其中表示第N颗卫星测量伪距噪声的强度。

设置状态量初始估计X₀和初始状态估计误差协方差P₀；本算法中用最小二乘算法估计滤波状态初始值。

滤波时刻k清零，将自适应因子赋值为1，即λ_i，k＝1。

S3：运用UKF滤波算法对状态量X滤波；

301：初始状态采样；

在计数器count＝0时，进行初始状态采样，得到初始状态sigma样本点及权重因子；

通过初始状态估计X₀和初始状态估计协方差矩阵P₀，根据对称采样策略，进行初始状态sigma样本点采样，得到初始状态sigma样本点及相应权重因子如公式(6)和(7)所示。

${\mathrm{\χ}}_0={\hat{X}}_0$

${\mathrm{\χ}}_i={\hat{X}}_0+{\left(\sqrt{(L+\mathrm{\λ})P_0}\right)}_i,i=1,...,L---\left(6\right)$

${\mathrm{\χ}}_i={\hat{X}}_0-{\left(\sqrt{(L+\mathrm{\λ})P_0}\right)}_{i-L},i=L+1,...,2L$

$W_0^{\left(m\right)}=\mathrm{\λ}/(L+\mathrm{\λ})$

$W_0^{\left(c\right)}=\mathrm{\λ}/(L+\mathrm{\λ})+(1-{\mathrm{\α}}^2+\mathrm{\β})---\left(7\right)$

$W_i^{\left(m\right)}=W_i^{\left(c\right)}=1/\left\{2(L+\mathrm{\λ})\right\},i=1,...,2L$

其中，状态量维数L，采样维数2L+1；λ＝α²(L+κ₁)-L表示采样尺度参数；α表示样本点χ_i在采样附近的遍布范围；κ₁通常设置为0或者3-L；对于状态量高斯分布 $\mathrm{\β}=2;{\left(\sqrt{(L+\mathrm{\λ})P_0}\right)}_i$ 表示矩阵平方根的第i列。

302：一步预测状态采样；

根据所获得的初始状态sigma样本点，对一步预测状态及一步预测状态协方差矩阵进行估计；具体形式如公式(8)、(9)和(10)所示。

χ_k|k-1＝f(χ_k-1)        (8)

${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i{\mathrm{\χ}}_{i,k|k-1}---\left(9\right)$

$P_{k|k-1}={\mathrm{\λ}}_k\{\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i{[{\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}][{\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}]}^T+Q_k\}---\left(10\right)$

303：一步预测状态二次采样；

对上述步骤获得的一步预测状态和一步预测协方差矩阵进行二次采样，得到二次采样的一步预测状态sigma样本点及权重因子；

利用最优采样理论，根据对称采样策略对一步预测状态和一步预测协方差P_k|k-1进行二次采样，再次得到一步预测状态sigma样本点及对应权重因子，具体公式如(11)和(12)所示。

$P_{k|k-1}{S}_{k|k-1}{S}_{k|k-1}^T---\left(11\right)$

${\mathrm{\χ}}_{k|k-1}=[{\hat{x}}_{k|k-1},{\hat{x}}_{k|k-1}+\sqrt{(L+\mathrm{\κ})}{S}_{k|k-1},{\hat{x}}_{k|k-1}-\sqrt{(L+\mathrm{\κ})}{S}_{k|k-1}]---\left(12\right)$

其中，S_k|k-1表示矩阵P_k|k-1的平方根矩阵，χ_k|k-1的每列分别表示一个采样向量点χ_i，k|k-1。

304：输出一步预测；

根据一步预测状态的二次采样输出观测量一步预测，之后进行观测量自协方差矩阵、观测量和状态量之间互协方差矩阵估计；

由一步预测状态的二次采样，输出观测量一步预测，具体公式如公式(13)和(14)所示。

ζ_k|k-1 ⁱ＝h_k(χ_k|k-1 ⁱ)i＝0，1，...，2L    (13)

${\hat{z}}_{k|k-1}=\underset{i=0}{\overset{2L}{\mathrm{\Σ}}}{W}_i^{\left(m\right)}{{\mathrm{\ζ}}_{k|k-1}}^i---\left(14\right)$

其中，y＝h(x)表示观测量与状态量之间的函数关系。

输出观测量一步预测后，对观测量自协方差矩阵和观测量与状态量的互协方差矩阵进行估计，具体如公式(15)、(16)所示。

$P_{z_k{z}_k}={\mathrm{\λ}}_k\{\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{[{\mathrm{\ζ}}_{i,k|k-1}-{\hat{z}}_{k|k-1}][{\mathrm{\ζ}}_{i,k|k-1}-{\hat{z}}_{k|k-1}]}^T+R\}---\left(15\right)$

$P_{x_k{z}_k}={\mathrm{\λ}}_k\left\{\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{W}_i{[{\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}][{\mathrm{\ζ}}_{i,k|k-1}-{\hat{z}}_{k|k-1}]}^T\right\}---\left(16\right)$

所述观测量自协方差矩阵作为滤波信息理论协方差矩阵M_k。

305：卡尔曼增益求解；

通过获得的观测量自协方差矩阵和观测量与状态量的互协方差矩阵，求解卡尔曼Kalman增益矩阵K，从而完成状态量的更新和状态量估计协方差的更新，具体如公式(17)～(19)所示。

$K=P_{x_k{z}_k}{P}_{z_k{z}_k}^{-1}---\left(17\right)$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K(z_k-{\hat{z}}_{k|k-1})---\left(18\right)$

$P_k=P_{k|k-1}-{\mathrm{KP}}_{z_k{z}_k}{K}^T---\left(19\right)$

S4：采用模糊逻辑系统求解强跟踪算法中的软化因子epsilon；模糊逻辑系统是以模糊集合为基础，最早提出用于系统控制，从而解决系统建模不准确带来的滤波器发散问题。一个完成的模糊逻辑系统由模糊化、模糊规则、模糊推理和去模糊化四个部分构成(参见San-Tong Zhang and Xue-Ye Wei.FuzzyAdaptive Kalman Filtering for DR/GPS IEEE.Proceedings of the secondInternational Conference on Machine Learning and Cybemetics，xi’an，2-5Nov.2003.b)。

强跟踪算法中的软化因子ε用于调节状态估计精度和状态跟踪能力。当接收机运动剧烈时，ε值应该减小，以减弱状态模型不准确导致的滤波器不稳定；反之当接收机运动平稳时，应适当增大ε，用于提高状态量的估计精度。参见图4所示，其包如下步骤：

401：获得滤波量测信息向量；

一次滤波更新结束后，获得滤波量测信息向量v_k；

402：模糊逻辑系统输入；

通过滤波量测信息向量v_k，求解模糊逻辑系统输入r_i；

其中，滤波量测信息向量v_k单步协方差矩阵的迹与单步理论信息方差阵的迹之比r_i作为模糊逻辑系统的输入，用以衡量i时刻滤波器的工作稳定程度，如公式(20)所示。当r_i≥1时，可认为滤波器开始发散；反之r_i＜1时，滤波器工作正常。

$r_i=\frac{z^T\left(k\right)z\left(k\right)}{\mathrm{tr}\left(P_{\mathrm{ZKZK}}\right)}---\left(20\right)$

403：模糊逻辑系统求解；

根据模糊逻辑系统输入r_i的变化规律，进行模糊逻辑系统的模糊化过程，设定模糊规则，进行模糊推理，去模糊化过程，实现软化因子输出。

所述模糊化过程包括划分模糊几何、建立模型系统输入参量隶属度函数，在模糊逻辑系统中，输入参量隶属度函数选取为三角形函数，但不局限于三角形函数，评价模糊子集合划分为{A，B，C}＝{good，normal，bad}。

模糊规则采用一阶T-S模型、IF-THEN形式但不局限于此，规则如下所示：

①IFr_i∈good     THENε等于r_i+15；

②IFr_i∈normal   THENε等于2*r_i+4；

③IFr_i∈bad      THENε等于1。

所述去模糊化过程或者模糊化输出由相应的A，B，C三个模糊子集合输出进行加权输出。假设P_i，k，k＝1，2，3为样本r_i输入FLAS系统时集合k的隶属度，y_i，k，k＝1，2，3为样本r_i输入时分属集合k的输出，可得模糊逻辑系统总的输出如公式(21)所示。

$\mathrm{\ϵ}=Y=\underset{k=1}{\overset{3}{\mathrm{\Σ}}}{P}_{i,k}{y}_{i,k}---\left(21\right)$

S5：强跟踪自适应算法中自适应因子λ_i，k求解；

参见图5所示，强跟踪自适应算法中自适应因子λ_i，k求解包括以下步骤：

501：由获得的滤波信息向量v_k，对滤波信息真实协方差进行估计；

在一次滤波更新结束后，在计数器k＝1时，获得滤波信息向量v_k；

滤波信息真实协方差矩阵估计采用了新型的递推算法，其克服了传统方法求解过程中对历史信息取平均，而直接采用了当前历元信息，更能敏感地反应当前观测历元动力学模型误差的现状；具体形式如公式(22)所示。

$V_k=\left\{\begin{array}{cc}\frac{1}{2}{v}_0{v_0}^T,& k=0\\ \frac{{\mathrm{\λ}}_{i,k-1}{v}_k{v_k}^T}{1+{\mathrm{\λ}}_{i,k-1}}& k\≥1\end{array}\right.---\left(22\right)$

所述强跟踪算法中滤波信息向量真实协方差的估计求解步骤如下：

一次滤波更新结束后，获得滤波信息向量v_k；

初始化滤波信息向量协方差，当计数器count＝0时，当计数器count≥1时， $V_k=\frac{{\mathrm{\λ}}_{i,k-1}{v}_k{v}_k^T}{1+{\mathrm{\λ}}_{i,k-1}}.$

502：通过滤波信息真实协方差求解包含信息协方差V_k和观测噪声估计的自适应矩阵N_k，如公式(23)所示；

N_k＝ηV_k-εR_k        (23)

503：由自适应矩阵N_k和信息理论协方差矩阵M_k求解自适应强跟踪因子中间参数C_i，k，如公式(24)所示；其中，信息理论协方差矩阵M_k即观测量自协方差矩阵，见公式(15)；

$C_k=\frac{\mathrm{tr}\left(N_k\right)}{\mathrm{tr}\left(M_k\right)}---\left(24\right)$

504：通过对中间参数C_i，k判定，对自适应因子λ_i，k赋值，如公式(25)所示。

${\mathrm{\λ}}_{i,k}=\left\{\begin{array}{cc}{C}_k& C_k>1\\ 1& C_k\≤1\end{array}\right.---\left(25\right)$

S6：历元时刻递1，count＝count+1时，读取下一时刻观测伪距，返回步骤(3)继续执行，直至结束。

以上公开的仅为本发明的具体实施例，但本发明并非局限于此，对于本领域的普通技术人员来说，在不脱离本发明原理的前提下，做出的变形应视为属于本发明保护范围。

Claims (2)

1.一种基于模糊逻辑的自适应强跟踪UKF定位滤波算法，其特征在于，包括： 

（1）建立定位滤波模型；所述定位滤波模型包括： 

针对低动态环境，根据接收机的位置、速度、时钟钟差和频差建立的常速CV模型；针对高动态环境，根据接收机的位置、速度、加速度、时钟钟差和频差建立的常加速度CA模型； 

（2）设定滤波器初始参数；所述设定滤波器初始参数包括：设置系统噪声矩阵Q(t)和观测噪声矩阵R；设置状态量初始估计X₀和初始状态估计误差协方差P₀；滤波时刻k清零，将自适应因子赋值1，即自适应因子λ_i,k＝1； 

（3）运用UKF滤波算法对状态量进行滤波；所述运用UKF滤波算法对状态量滤波的步骤包括：初始状态采样：初始状态量采样，得到初始状态sigma样本点及对应权重因子；一步预测状态采样：根据所获得的初始状态sigma样本点，对一步预测状态和一步预测状态协方差矩阵进行估计；一步预测状态二次采样：对获得的一步预测状态及一步预测状态协方差矩阵进行二次采样，得到二次采样的一步预测状态sigma样本点及对应权重因子； 

输出一步预测：根据一步预测状态的二次采样输出观测量一步预测，之后进行观测量自协方差矩阵、观测量和状态量之间互协方差矩阵估计； 

卡尔曼增益求解：通过获得的观测量自协方差矩阵和观测量与状态量的互协方差矩阵，求解卡尔曼增益矩阵，实现状态量估计更新和状态量估计协方差更新；

（4）运用模糊逻辑系统求解强跟踪自适应算法中的软化因子；所述运用模糊逻辑系统求解强跟踪自适应算法中的软化因子的步骤包括：获得滤波信息向量v_k；模糊逻辑系统输入：通过滤波信息向量v_k，求解模糊逻辑系统输入r_i,其中模糊逻辑系统输入r_i为滤波信息向量v_k单步协方差矩阵的迹与单步理论信息 方差阵的迹之比；模糊逻辑系统求解：根据模糊逻辑系统输入r_i的变化规律，进行模糊逻辑系统的模糊化过程，设定模糊规则，进行模糊推理，去模糊化过程，实现软化因子输出； 

（5）强跟踪自适应算法中自适应因子求解；所述强跟踪自适应算法中自适应因子λ_i,k求解的步骤包括：由滤波信息向量v_k对滤波信息真实协方差V_k进行估计求解；求解包含滤波信息协方差V_k和观测噪声估计的自定应矩阵N_k；由自定应矩阵N_k和信息理论协方差矩阵M_k求解自适应因子中间参数C_i,k；通过对中间参数C_i,k进行判定来对自适应因子λ_i,k赋值； 

（6）历元时刻递加1，读取下一时刻观测，返回步骤（4），直至结束。 

2.根据权利要求1所述的基于模糊逻辑的自适应强跟踪UKF定位滤波算法，其特征在于，所述模糊化过程中输入参量隶属度函数包括三角形函数，所述模型规则包括一阶T-S模型、IF-THEN形式。