CN104539265A

CN104539265A - Self-adaptive UKF (Unscented Kalman Filter) algorithm

Info

Publication number: CN104539265A
Application number: CN201410691143.1A
Authority: CN
Inventors: 何俊; 张清华; 孙国玺; 肖明; 熊建斌; 丘海健
Original assignee: Guangdong University of Petrochemical Technology
Current assignee: Guangdong University of Petrochemical Technology
Priority date: 2014-11-25
Filing date: 2014-11-25
Publication date: 2015-04-22

Abstract

The invention discloses an adaptive UKF filtering algorithm, which is an algorithm combining the Kalman filtering algorithm based on the maximum likelihood criterion and the adaptive UKF filtering algorithm based on the maximum a posteriori estimation. The covariance is estimated in real time, and then the average is taken to obtain an estimated value with a better tracking effect on the true value of the prior covariance, thereby improving the filtering accuracy and filtering stability.

Description

An Adaptive UKF Filtering Algorithm

技术领域 technical field

本发明涉及一种自适应UKF滤波算法，具体地说，涉及一种结合极大似然估计和最大后验估计的自适应UKF滤波算法。 The invention relates to an adaptive UKF filtering algorithm, in particular to an adaptive UKF filtering algorithm combining maximum likelihood estimation and maximum a posteriori estimation.

背景技术 Background technique

为了解决非线性情况下卡尔曼滤波及之后的扩展卡尔曼滤波在估计中存在较大误差甚至可能引起滤波器发散的问题,近年来,Julier和Uhlman提出了基于多元函数代表点思想的Unscented Kalman Filter(UKF)方法，UKF算法精度明显高于EKF算法且计算量又明显小于EKF算法，但是UKF算法存在一个主要的问题是噪声统计特性未知时，UKF滤波精度下降甚至发散，为解决这个问题，各国学者提出了很多解决的方法，其中文献[7]提出了一种根据极大后验估计原理和指数加权，推导出一种含有渐消因子的次优无偏MAP时变噪声统计估计器的算法，该算法具有自适应能力且递推公式简单，易于工程实现的特点，但是该算法需要有足够的数据，才能达到比较理想的效果。 In order to solve the problem that the Kalman filter and the subsequent extended Kalman filter have large errors in estimation and may even cause filter divergence in nonlinear conditions, in recent years, Julier and Uhlman proposed the Unscented Kalman Filter based on the idea of representative points of multivariate functions. (UKF) method, the accuracy of the UKF algorithm is significantly higher than that of the EKF algorithm and the amount of calculation is significantly smaller than that of the EKF algorithm. However, there is a main problem with the UKF algorithm that when the statistical characteristics of the noise are unknown, the accuracy of the UKF filter decreases or even diverges. To solve this problem, various countries Scholars have proposed many solutions. Among them, literature [7] proposed an algorithm based on the principle of maximum a posteriori estimation and exponential weighting to derive a suboptimal unbiased MAP time-varying noise statistical estimator with fading factors , the algorithm has the characteristics of self-adaptive ability, simple recursive formula, and easy engineering implementation, but the algorithm needs enough data to achieve a relatively ideal effect.

发明内容 Contents of the invention

本发明的目的在于克服上述技术存在的缺陷，提供一种自适应UKF滤波算法，是一种将基于极大似然准则的卡尔曼滤波算法和基于最大后验估计的自适应UKF滤波算法结合到一起的算法，通过两种算法对协方差进行实时的估计，然后平均取中得到对先验协方差真实值跟踪效果更好的估计值，从而提高滤波精度及滤波稳定性。其具体技术方案为： The purpose of the present invention is to overcome the defective that above-mentioned technology exists, provide a kind of adaptive UKF filtering algorithm, be a kind of Kalman filtering algorithm based on maximum likelihood criterion and the adaptive UKF filtering algorithm based on maximum a posteriori estimation combined into The algorithm together uses the two algorithms to estimate the covariance in real time, and then averages to obtain an estimated value with a better tracking effect on the true value of the prior covariance, thereby improving the filtering accuracy and filtering stability. Its specific technical plan is:

一种自适应UKF滤波算法，包括以下步骤： A kind of adaptive UKF filter algorithm, comprises the following steps:

第一步：判断是否已经稳定收敛 Step 1: Determine whether it has converged stably

Q_k-Q_k-2≤H _Qk - _Qk-2≤H

R_k-R_k-2≤H R _k -R _k-2 ≤ H

其中，H为一个很小的正整数； Among them, H is a very small positive integer;

第二步：选出优值 Step 2: Select the best value

设两种算法的估计协方差分别为Q1_k、R1_k、Q2_k、R2_k，则 Suppose the estimated covariances of the two algorithms are Q1 _k , R1 _k , Q2 _k , R2 _k , then

${\overset{^^}{Q Q}}_{k k} = = \frac{{Q Q 11}_{k k} + + {Q Q 22}_{k k}}{22}$

${\overset{^^}{R R}}_{k k} = = \frac{{R R 11}_{k k} + + {R R 22}_{k k}}{22}$

式中，就是协方差估计值。 In the formula, is the covariance estimate.

与现有技术相比，本发明的有益效果为： Compared with prior art, the beneficial effect of the present invention is:

本发明所述基于最大后验估计的UKF自适应算法相对于传统UKF算法，克服传统UKF算法存在的噪声统计特性未知情况下滤波发散的问题，而本文中提出的结合最大似然估计和最大后验估计优缺点，选择最佳的噪声统计特性估计的UKF自适应算法，在仿真结果中，可以看出其在滤波精度上确实提高了。 Compared with the traditional UKF algorithm, the UKF adaptive algorithm based on maximum a posteriori estimation described in the present invention overcomes the problem of filter divergence in the case of unknown noise statistical characteristics of the traditional UKF algorithm. According to the advantages and disadvantages of estimation, the best UKF adaptive algorithm for noise statistical characteristic estimation is selected. From the simulation results, it can be seen that its filtering accuracy has indeed improved.

附图说明 Description of drawings

图1是文[7]过程噪声均值自适应估计曲线图； Fig. 1 is the curve diagram of self-adaptive estimation of process noise mean in [7];

图2是文[7]过程噪声协方差自适应估计曲线图； Fig. 2 is a graph of adaptive estimation of process noise covariance in [7];

图3是本发明过程噪声协方差估计曲线图； Fig. 3 is a curve diagram of process noise covariance estimation of the present invention;

图4是文献[7]量测噪声均值自适应估计曲线图； Fig. 4 is the curve diagram of the self-adaptive estimation of the measurement noise mean in literature [7];

图5文献[7]量测噪声协方差自适应估计曲线图； Figure 5 Document [7] measurement noise covariance adaptive estimation curve;

图6本发明量测噪声协方差自适应估计曲线图。 Fig. 6 is a curve diagram of self-adaptive estimation of measurement noise covariance in the present invention.

具体实施方式 Detailed ways

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体实例进一步阐述本发明。 In order to make the technical means, creative features, goals and effects achieved by the present invention easy to understand, the present invention will be further described below in conjunction with specific examples.

1问题阐述 1 Problem statement

考虑如下所示的非线性离散系统： Consider the nonlinear discrete system shown below:

$\{\begin{matrix} {x x}_{k k} = = f f (({x x}_{k k - - 11})) + + {w w}_{k k - - 11} \\ {z z}_{k k} = = h h (({x x}_{k k})) + + {v v}_{k k} \end{matrix} - - - - - - ((11))$

其中,k≥0为离散时间变量；f(x_k-1)和h(x_k)分别为系统非线性状态函数和量测函数；x_k与z_k分别为系统n维状态向量和l维量测向量；w_k和v_k分别为n维系统噪声和l维量测噪声,并且具有以下统计特性： Among them, k≥0 is a discrete time variable; f(x _k-1 ) and h(x _k ) are the nonlinear state function and measurement function of the system respectively; x _k and z _k are the n-dimensional state vector and l-dimensional Measurement vector; w _k and v _k are n-dimensional system noise and l-dimensional measurement noise respectively, and have the following statistical properties:

$\{\begin{matrix} E E. [[{w w}_{k k}]] = = q q,, cov cov (({w w}_{k k},, {w w}_{j j})) = = Q Q {δ δ}_{k k,, j j} \\ E E. [[{v v}_{k k}]] = = r r,, cov cov (({v v}_{k k},, {v v}_{j j})) = = R R {δ δ}_{k k,, j j} \\ cov cov (({w w}_{k k},, {v v}_{j j})) = = 00 \end{matrix} - - - - - - ((22))$

在许多情况下，噪声均值都是非零均值的，因此，不妨令系统噪声和量测噪声为非零均值的高斯噪声且其均值分别为q和r.则： In many cases, the mean value of the noise is non-zero mean. Therefore, it is advisable to make the system noise and measurement noise Gaussian noise with a non-zero mean value and their mean values are q and r respectively. Then:

可见，μ_k和为互不相关的零均值高斯白噪声，且其方差分别为Q和R，故离散非线性系统(1)等价为： Visible, μ _k and is uncorrelated zero-mean Gaussian white noise, and its variances are Q and R respectively, so the discrete nonlinear system (1) is equivalent to:

2自适应UKF滤波算法 2 Adaptive UKF filtering algorithm

2.1噪声均值非零时的UKF算法 2.1 UKF algorithm when the noise mean is non-zero

步骤1.初始化系统变量 Step 1. Initialize system variables

${\overset{^^}{x x}}_{00} = = E E. (({x x}_{00})) - - - - - - ((55))$

${P P}_{00} = = E E. (((({x x}_{00} - - {\overset{^^}{x x}}_{00})) {(({x x}_{00} - - {\overset{^^}{x x}}_{00}))}^{T T})) - - - - - - ((66))$

步骤2.计算sigma点及对应权值 Step 2. Calculate sigma points and corresponding weights

由确定的采样策略，得到服从均值为方差为P_k-1的Sigma点{X_i,k-1}；其中，i＝0,1,2,…,2n，算子表示对称的Cholesky分解。 According to the determined sampling strategy, the obedience mean is Sigma point {X _i,k-1 } with variance P _k-1 ; where, i=0,1,2,…,2n, operator Represents a symmetric Cholesky decomposition.

$\{\begin{matrix} {X x}_{00,, k k - - 11} = = {x x}_{k k - - 11} \\ {X x}_{i i,, k k - - 11} = = {x x}_{k k - - 11} + + {((\sqrt{((n no + + 11)) {P P}_{k k - - 11}}))}_{i i},, i i = = 1,2 1,2,, . . . . . .,, n no \\ {X x}_{i i,, k k - - 11} = = {x x}_{k k - - 11} - - {((\sqrt{((n no + + 11)) {P P}_{K K - - 11}}))}_{i i},, i i = = n no + + 11,, n no + + 22,, . . . . . .,, 22 n no \end{matrix} - - - - - - ((77))$

Sigma点的对应权值如下 The corresponding weight of the Sigma point is as follows

$\{\begin{matrix} {w w}_{00}^{((m m))} = = \frac{11}{n no + + 11} \\ {w w}_{00}^{((c c))} = = \frac{11}{n no + + 11} + + 11 - - {α α}^{22} + + β β \\ {w w}_{i i}^{((m m))} = = {w w}_{i i}^{((c c))} = = \frac{0.5 0.5}{n no + + 11},, i i = = 1,2 1,2,, . . . . . .,, 22 n no \end{matrix} - - - - - - ((88))$

其中，α描叙了Sigma点在均值周围的延伸程度；β为调整参数，l＝α²(n+k)-n,k描述了系统分布信息。 Among them, α describes the extension degree of the Sigma point around the mean value; β is an adjustment parameter, l=α ² (n+k)-n, and k describes the system distribution information.

步骤3.时间更新 Step 3. Time update

根据步骤2得到的Sigma点X_i,k-1(i＝0,1,2,…,2n)及其对应的权值，使X_i,k-1通过非线性状态函数f(·)+q传播后为X_i,k|k-1，根据X_i,k|k-1可得到一步状态预测及其对应的误差协方差阵P_k|k-1 According to the Sigma point X _i,k-1 (i=0,1,2,…,2n) obtained in step 2 and its corresponding weights, let X _i,k-1 pass through the nonlinear state function f(·)+ After q propagation, it is Xi _,k|k-1 , and one-step state prediction can be obtained according to Xi _,k|k-1 And its corresponding error covariance matrix P _k|k-1

X_i,k|k-1＝f(X_i,k-1)+q，i＝0,1,…,2n (9) X _i,k|k-1 = f(X _i,k-1 )+q, i=0,1,...,2n (9)

${x x}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 11}^{22 n no} {w w}_{i i}^{((m m))} \times \times {X x}_{i i,, k k | | k k - - 11} - - - - - - ((1010))$

${P P}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {x x}_{k k | | k k - - 11})) \times \times {(({X x}_{i i,, k k | | k k - - 11} - - {x x}_{k k | | k k - - 11}))}^{T T} + + Q Q - - - - - - ((1111))$

步骤4.计算sigma点及对应权值 Step 4. Calculate sigma points and corresponding weights

再由确定的采样策略，得到服从均值为方差为P_k|k-1的Sigma点{X_i,k|k-1},i＝0,…,2n Then by the determined sampling strategy, the obedience mean is obtained Sigma point {X i, k| _{k-1 } with variance P k|} _k-1 , i=0,...,2n

$\{\begin{matrix} {X x}_{00,, k k | | k k - - 11} = = {x x}_{k k | | k k - - 11} \\ {X x}_{i i,, k k | | k k - - 11} = = {x x}_{k k | | k k - - 11} + + {((\sqrt{((n no + + 11)) {P P}_{k k | | k k - - 11}}))}_{i i},, i i = = 1,2 1,2,, . . . . . .,, n no \\ {X x}_{i i,, k k | | k k - - 11} = = {x x}_{k k | | k k - - 11} - - {((\sqrt{((n no + + 11)) {P P}_{k k | | k k - - 11}}))}_{i i},, i i = = n no + + 11,, n no + + 22,, . . . . . .,, 22 n no \end{matrix} - - - - - - ((1212))$

其对应的权值与步骤2中(8)式的一致。 Its corresponding weight value is consistent with the formula (8) in step 2.

步骤5.量测更新 Step 5. Measurement update

再根据步骤4中新得到的Sigam点X_i,k|k-1(i＝0,…,2n)及权值，令X_i,k|k-1通过非线性量测函数h(·)+r传播后得到z_i,k|k-1，再由z_i,k|k-1得到输出预测及自协方差阵和互协方差阵 Then according to the newly obtained Sigam points X _i,k|k-1 (i=0,...,2n) and weights in step 4, let X _i,k|k-1 pass the nonlinear measurement function h( ) After +r propagation, z _i,k|k-1 is obtained, and then the output prediction is obtained by z _i,k|k-1 and autocovariance matrix and cross-covariance matrix

z_i,k|k-1＝h(X_i,k|k-1)+r，i＝0,1,…,2n (13) z _i,k|k-1 = h(X _i,k|k-1 )+r, i=0,1,...,2n (13)

${\overset{^^}{z z}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((m m))} \times \times {z z}_{i i,, k k | | k k - - 11} - - - - - - ((1414))$

${P P}_{{\overset{^^}{z z}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11})) \times \times {(({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T} + + R R - - - - - - ((1515))$

${P P}_{{\overset{^^}{x x}}_{k k} {\overset{^^}{z z}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) \times \times {(({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T} - - - - - - ((1616))$

步骤6.滤波更新 Step 6. Filter updates

经过了时间和量测更新后，通过计算可得到滤波增益矩阵K_k和k时刻状态向量估计值及其对应的协方差阵P_k|k-1 After time and measurement updates, the filter gain matrix K _k and the estimated value of the state vector at time k can be obtained by calculation And its corresponding covariance matrix P _k|k-1

${ϵ ϵ}_{k k} = = {z z}_{k k} - - {\overset{^^}{z z}}_{k k | | k k - - 11} - - - - - - ((1717))$

${K K}_{k k} = = {P P}_{{\overset{^^}{x x}}_{k k} {\overset{^^}{z z}}_{k k}} {(({P P}_{{\overset{^^}{z z}}_{k k}}))}^{- - 11} - - - - - - ((1818))$

${\overset{^^}{x x}}_{k k} = = {\overset{^^}{x x}}_{k k | | k k - - 11} + + {K K}_{K K} {ϵ ϵ}_{K K} - - - - - - ((1919))$

${P P}_{k k | | k k - - 11} = = {P P}_{k k | | k k - - 11} - - {K K}_{k k} {P P}_{{\overset{^^}{z z}}_{k k}} {(({K K}_{k k}))}^{T T} - - - - - - ((2020))$

2.2自适应UKF算法 2.2 Adaptive UKF algorithm

2.2.1常值噪声估计器 2.2.1 Constant Noise Estimator

由文献[7]知基于极大后验估计原理，得到次优无偏MAP常值噪声统计估计器： According to literature [7], based on the principle of maximum a posteriori estimation, a suboptimal unbiased MAP constant noise statistical estimator is obtained:

${\overset{^^}{q q}}_{k k} = = \frac{11}{k k} [[((k k - - 11)) {\overset{^^}{q q}}_{k k - - 11} + + {\overset{^^}{x x}}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{m m} \times \times f f (({X x}_{i i,, k k - - 11}))]] - - - - - - ((21 twenty one))$

${\overset{^^}{r r}}_{k k} = = \frac{11}{k k} [[((k k - - 11)) {\overset{^^}{r r}}_{k k - - 11} + + {\overset{^^}{z z}}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{m m} \times \times h h (({X x}_{i i,, k k | | k k - - 11}))]] - - - - - - ((22 twenty two))$

${\overset{^^}{R R}}_{k k} = = \frac{11}{k k} [[((k k - - 11)) {\overset{^^}{R R}}_{k k - - 11} + + {ϵ ϵ}_{k k} {ϵ ϵ}_{k k}^{T T} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{c c} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11})) {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T}]] - - - - - - ((23 twenty three))$

${\overset{^^}{Q Q}}_{k k} = = \frac{11}{k k} [[((k k - - 11)) {\overset{^^}{Q Q}}_{k k - - 11} + + {K K}_{k k} {ϵ ϵ}_{k k} {ϵ ϵ}_{k k}^{T T} {K K}_{k k}^{T T} + + {P P}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{c c} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11}))}^{T T}]] - - - - - - ((24 twenty four))$

其中，分别代表k时刻的系统噪声均值估计值，量测噪声均值估计值，系统噪声协方差估计值，量测噪声协方差估计值。 in, Represent the estimated value of the mean value of the system noise, the estimated value of the mean value of the measurement noise, the estimated value of the covariance of the system noise, and the estimated value of the covariance of the measurement noise at time k, respectively.

2.2.2渐消因子 2.2.2 Evanescent factor

假设加权系数{β_i}满足：β_i＝β_i-1b且其中0<b<1为遗忘因子。 Suppose the weighting coefficient {β _i } satisfies: β _i = β _i-1 b and Among them, 0<b<1 is the forgetting factor.

根据等比数列求和公式和权系数的约束条件，得到权系数与时间{β_i}相关的一般表达式为 According to the geometric sequence summation formula and the constraint conditions of the weight coefficient, the general expression related to the weight coefficient and time {β _i } is obtained as

$\{\begin{matrix} {β β}_{i i} = = d d ((k k)) {b b}^{i i - - 11,,} i i = = 11,, . . . . . .,, k k \\ d d ((k k)) = = \frac{11 - - b b}{11 - - {b b}^{k k}} \end{matrix} - - - - - - ((2525))$

2.2.3带时变噪声估计器的自适应UKF算法 2.2.3 Adaptive UKF algorithm with time-varying noise estimator

由文献[10]知用渐消因子d(k)代替式(21)和(24)中的权系数1/k，便可得出渐消记忆时变噪声统计估计器： It is known from literature [10] that the weight coefficient 1/k in equations (21) and (24) is replaced by the fading factor d(k), and the statistical estimator of fading memory time-varying noise can be obtained:

${\overset{^^}{q q}}_{k k} = = ((11 - - {d d}_{k k})) {\overset{^^}{q q}}_{k k - - 11} + + {d d}_{k k} (({\overset{^^}{x x}}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{m m} \times \times f f (({X x}_{i i,, k k - - 11})))) - - - - - - ((2626))$

${\overset{^^}{r r}}_{k k} = = ((11 - - {d d}_{k k})) {\overset{^^}{r r}}_{k k - - 11} + + {d d}_{k k} (({\overset{^^}{z z}}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{m m} \times \times h h (({X x}_{i i,, k k | | k k - - 11})))) - - - - - - ((2727))$

${\overset{^^}{R R}}_{k k} = = ((11 - - {d d}_{k k})) {\overset{^^}{R R}}_{k k - - 11} + + {d d}_{k k} (({ϵ ϵ}_{k k} {ϵ ϵ}_{k k}^{T T} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{c c} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11})) {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T})) - - - - - - ((2929))$

${\overset{^^}{Q Q}}_{k k} = = ((11 - - {d d}_{k k})) {\overset{^^}{Q Q}}_{k k - - 11} + + {d d}_{k k} (({K K}_{k k} {ϵ ϵ}_{k k} {ϵ ϵ}_{k k}^{T T} {K K}_{k k}^{T T} + + {P P}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{c c} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11}))}^{T T})) - - - - - - ((3030))$

相应地，结合式(5)和(20)和式(26)和(29)便可以得出自适应UKF滤波算法： Correspondingly, combining formulas (5) and (20) and formulas (26) and (29), the adaptive UKF filtering algorithm can be obtained:

${\overset{^^}{x x}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((m m))} \times \times f f (({X x}_{i i,, k k - - 11})) + + {\overset{^^}{q q}}_{k k - - 11} - - - - - - ((3131))$

${P P}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) \times \times {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11}))}^{T T} + + {\overset{^^}{Q Q}}_{k k - - 11} - - - - - - ((3232))$

${\overset{^^}{z z}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((m m))} \times \times h h (({X x}_{i i,, k k | | k k - - 11})) + + {\overset{^^}{r r}}_{k k} - - - - - - ((3333))$

${P P}_{{\overset{^^}{z z}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11})) \times \times {(({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T} + + {\overset{^^}{R R}}_{k k} - - - - - - ((3535))$

${P P}_{{\overset{^^}{x x}}_{k k} {\overset{^^}{z z}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) \times \times {(({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11}))}^{T T} - - - - - - ((3636))$

${K K}_{k k} = = {P P}_{{\overset{^^}{x x}}_{k k} {\overset{^^}{z z}}_{k k}} {(({P P}_{{\overset{^^}{z z}}_{k k}}))}^{- - 11} - - - - - - ((3737))$

${\overset{^^}{x x}}_{k k} = = {\overset{^^}{x x}}_{k k | | k k - - 11} + + {K K}_{k k} {ϵ ϵ}_{k k} - - - - - - ((3838))$

${P P}_{k k | | k k} = = {P P}_{k k | | k k - - 11} - - {K K}_{k k} {P P}_{{\overset{^^}{z z}}_{k k}} {(({K K}_{k k}))}^{T T} - - - - - - ((3939))$

3极大似然准则和Q、R阵在线估计调整 3 Maximum Likelihood Criterion and Q, R Array Online Estimation Adjustment

根据文献[4]提出的思想利用矩阵求导的公式和UKF滤波公式结合推导出将基于极大似然估计准则的R和Q阵的估计和调整问题转变成1个确定以及a＝a(Q,R)(表示需要估计的噪声方差阵)的问题的估计公式。 According to the idea proposed in literature [4], the formula of matrix derivation and UKF filter formula are used to deduce that the estimation and adjustment of R and Q matrices based on the maximum likelihood estimation criterion are transformed into a determination And the estimation formula of the problem of a=a(Q, R) (representing the noise variance matrix that needs to be estimated).

不妨设系统新息方差为： May wish to set the system innovation variance as:

${P P}_{{V V}_{k k}} = = \frac{11}{N N} {Σ Σ}_{i i = = k k - - N N + + 11}^{k k} {ϵ ϵ}_{i i} {ϵ ϵ}_{i i}^{T T} - - - - - - ((4040))$

式中，新息： $ϵ_{k} = Z_{k} - h ({\hat{x}}_{k | k - 1}) - - - (41),$ 估计窗大小N. In the formula, new information: $ϵ_{k} = Z_{k} - h ({\hat{x}}_{k | k - 1}) - - - (41),$ Estimated window size N.

极大似然估计是从系统量测出现概率最大的角度实现对Q、R阵的实时估计与调整，基于极大似然条件下的目标函数J(a)： The maximum likelihood estimation is to realize the real-time estimation and adjustment of the Q and R arrays from the point of view of the maximum occurrence probability of the system measurement, based on the objective function J(a) under the maximum likelihood condition:

$J J ((a a)) = = {Σ Σ}_{i i = = k k - - N N + + 11}^{k k} | | {P P}_{{V V}_{k k}} | | + + {Σ Σ}_{i i = = k k - - N N + + 11}^{k k} {ϵ ϵ}_{i i} {P P}_{{V V}_{k k}}^{- - 11} {ϵ ϵ}_{i i}^{T T} - - - - - - ((4242))$

式中，|·|表示行列式。 In the formula, |·| represents the determinant.

对a求偏导将自适应滤波问题转化为新息方差对a求导的问题： Taking partial derivatives for a transforms the adaptive filtering problem into a question of taking the derivative of innovation variance for a:

${Σ Σ}_{i i = = k k - - N N + + 11}^{k k} {{tr tr {{{P P}_{{V V}_{k k}}^{- - 11} \frac{&PartialD; &PartialD; {P P}_{{V V}_{k k}}}{&PartialD; &PartialD; a a}}} - - {ϵ ϵ}_{i i}^{T T} {P P}_{{V V}_{k k}}^{- - 11} \frac{&PartialD; &PartialD; {P P}_{{V V}_{k k}}}{&PartialD; &PartialD; a a} {P P}_{{V V}_{k k}}^{- - 11} {ϵ ϵ}_{i i}}} = = 00 - - - - - - ((4343))$

式中，tr表示取矩阵的迹。 In the formula, tr means to take the trace of the matrix.

根据标准无迹卡尔曼滤波方程得： According to the standard unscented Kalman filter equation:

${P P}_{{\overset{^^}{z z}}_{k k}} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11})) \times \times {(({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T} + + {R R}_{k k} - - - - - - ((4444))$

${P P}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) \times \times {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11}))}^{T T} + + {Q Q}_{k k} - - - - - - ((4545))$

3.1R阵估计 3.1 R matrix estimation

为得到R阵的实时估计表达式，不妨假设Q阵为完全已知，则式(44)a求导与式(43)联合化简得： In order to obtain the real-time estimation expression of the R matrix, it may be assumed that the Q matrix is completely known, then the joint derivation of formula (44)a and formula (43) can be simplified as follows:

${Σ Σ}_{i i = = k k - - N N + + 11}^{k k} tr tr [[{P P}_{{V V}_{k k}}^{- - 11} (({P P}_{{V V}_{k k}} - - {ϵ ϵ}_{i i} {ϵ ϵ}_{i i}^{T T})) {P P}_{{V V}_{k k}}^{- - 11}]] = = 00 - - - - - - ((4343))$

由新息序列的协方差矩阵为正定阵,得到在整个估计窗内，上式成立的条件为： $P_{V_{k}} = \frac{1}{N} Σ_{i = k - N + 1}^{k} ϵ_{i} ϵ_{i}^{T} - - - (44)$ From the fact that the covariance matrix of the innovation sequence is a positive definite matrix, it can be obtained that within the entire estimation window, the conditions for the establishment of the above formula are: $P_{V_{k}} = \frac{1}{N} Σ_{i = k - N + 1}^{k} ϵ_{i} ϵ_{i}^{T} - - - (44)$

故观测噪声协方差的实时估计值： Therefore, the real-time estimate of the observation noise covariance is:

${\overset{^^}{R R}}_{k k} = = \frac{11}{N N} {Σ Σ}_{i i = = k k - - N N + + 11}^{k k} {ϵ ϵ}_{i i} {ϵ ϵ}_{i i}^{T T} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11})) \times \times {(({z z}_{i i,, k k | | k k - - 11} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))}^{T T} - - - - - - ((4545))$

3.2Q阵估计 3.2 Q matrix estimation

同理为得到Q阵的实时估计表达式，不妨假设R阵为完全精确已知。 Similarly, in order to obtain the real-time estimation expression of the Q matrix, it may be assumed that the R matrix is completely and precisely known.

设状态估计误差为： Let the state estimation error be:

$Δ Δ {X x}_{k k} = = {\overset{^^}{X x}}_{k k} - - {\overset{^^}{X x}}_{k k,, k k - - 11} = = {K K}_{k k} {ϵ ϵ}_{k k} - - - - - - ((4646))$

则联合式(40)与(42)，结合标准无迹卡尔曼滤波方程得过程噪声协方差的实时估计值： Then combine equations (40) and (42), and combine the standard unscented Kalman filter equation to obtain the real-time estimated value of the process noise covariance:

${\overset{^^}{Q Q}}_{k k} = = \frac{11}{N N} {Σ Σ}_{i i = = k k - - N N + + 11}^{k k} {ΔX ΔX}_{k k} Δ Δ {X x}_{k k}^{T T} + + {P P}_{k k} - - {Σ Σ}_{i i = = 00}^{22 n no} {w w}_{i i}^{((c c))} \times \times (({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11})) \times \times {(({X x}_{i i,, k k | | k k - - 11} - - {\overset{^^}{x x}}_{k k | | k k - - 11}))}^{T T} - - - - - - ((4747))$

4改进的自适应UKF滤波算法 4 Improved Adaptive UKF Filtering Algorithm

4.1估计窗范围 4.1 Estimated window range

估计窗的确定有一定的风险,体现在估计窗过大,滤波结果的无偏性较好,但可能导致系统模型的动态性能表现较差；估计窗过小,虽然能够较好的反映系统模型的变化,但是不能充分应用观测值所包含的信息,不能保证滤波过程的无偏性,严重的情况甚至导致滤波的发散，因此，估计窗大小的确定要具体问题具体分析，结合实际情况，尽可能的找出最好的估计窗范围。 The determination of the estimation window has certain risks, which is reflected in the fact that the estimation window is too large, and the unbiasedness of the filtering results is better, but it may lead to poor dynamic performance of the system model; the estimation window is too small, although it can better reflect the system model However, the information contained in the observations cannot be fully applied, and the unbiasedness of the filtering process cannot be guaranteed. In severe cases, it may even cause the divergence of the filtering. Find the best estimated window range possible.

4.2滤波发散的原因和选择最优Q、R阵的估计值的确定规则 4.2 Causes of filter divergence and determination rules for selecting the estimated values of optimal Q and R arrays

由于噪声的先验统计特性未知时变，使得噪声实际协方差偏离真实值，从而导致状态估计误差增大，这就是滤波发散的根本原因。 Due to the unknown time-varying priori statistical characteristics of the noise, the actual covariance of the noise deviates from the true value, which leads to an increase in the state estimation error, which is the root cause of the filter divergence.

UKF采用kalman滤波框架,kalman滤波为增长记忆滤波，初始阶段很大，可以充分的利用量测信息，有较强的校正作用，使估计值快速的接近估计状态，但是随着时间的推移，使得逐渐减小，滤波过程的增益K_k也愈来愈小，使反映真实状态的新的测量数据在估计中修正作用越来越弱，并形成数据饱和现象，会引起滤波发散。 UKF adopts the kalman filter framework, and the kalman filter is a growth memory filter. The initial stage is very large, and the measurement information can be fully utilized. It has a strong correction effect and makes the estimated value quickly approach the estimated state. However, as time goes by, it makes Gradually decreases, the gain K _k of the filtering process is also getting smaller and smaller, so that the correction effect of the new measurement data reflecting the real state becomes weaker and weaker in the estimation, and the phenomenon of data saturation is formed, which will cause the filtering divergence.

根据文献[7]滤波性能分析和上面对滤波发散原因分析，最优的Q、R阵估计值是尽可能的接近收敛于真实值，因此，我们要选择最接近真实值且收敛性更稳的Q、R阵估计值。噪声协方差未知且初值不一定准确，故噪声统计估计器需要一定的时间估计适应，才能达到稳定。 According to the filter performance analysis in literature [7] and the above analysis of the reasons for filter divergence, the optimal Q and R matrix estimates are as close as possible to converge to the real value. Therefore, we should choose the closest to the real value and the convergence is more stable. The estimated value of the Q, R matrix. The noise covariance is unknown and the initial value is not necessarily accurate, so the noise statistical estimator needs a certain amount of time to estimate and adapt to achieve stability.

所以，我们可以通过以下几步来完成Q、R阵的估计： Therefore, we can complete the estimation of Q and R matrix through the following steps:

Q_k-Q_k-2≤H (46) Q _k -Q _k-2 ≤ H (46)

R_k-R_k-2≤H (47) R _k -R _k-2 ≤ H (47)

其中，H为一个很小的正整数 Among them, H is a very small positive integer

第二步：选出优值 Step 2: Select the best value

不妨设，两种算法的估计协方差分别为Q1_k、R1_k、Q2_k、R2_k，则 It may be assumed that the estimated covariances of the two algorithms are Q1 _k , R1 _k , Q2 _k , and R2 _k respectively, then

${\overset{^^}{Q Q}}_{k k} = = \frac{{Q Q 11}_{k k} + + {Q Q 22}_{k k}}{22} - - - - - - ((4848))$

${\overset{^^}{R R}}_{k k} = = \frac{{R R 11}_{k k} + + {R R 22}_{k k}}{22} - - - - - - ((4949))$

4.3改进的自适应UKF滤波算法实现： 4.3 Improved adaptive UKF filtering algorithm implementation:

1)由式(5)和(20)实现经典UKF滤波。 1) The classic UKF filtering is realized by equations (5) and (20).

2)由式(26)和(27)实现噪声均值的估计。 2) Estimation of noise mean is realized by equations (26) and (27).

3)由式(44)和(46)和(28)和(29)实现噪声协方差估计，按照4.2提出的规则比较两者的结果，选择最佳的作为噪声的估计协方差。 3) Estimate the noise covariance by formulas (44) and (46) and (28) and (29), compare the results of the two according to the rules proposed in 4.2, and choose the best one as the estimated covariance of the noise.

4)将2)3)得到的噪声统计特性代替1)中相应的噪声统计特性。 4) Replace the noise statistical characteristics obtained in 2) and 3) with the corresponding noise statistical characteristics in 1).

5仿真分析 5 simulation analysis

本文采用文献[5]的强非线性高斯系统模型来验证改进后的自适应UKF滤波算法的有效性： In this paper, the strong nonlinear Gaussian system model of literature [5] is used to verify the effectiveness of the improved adaptive UKF filtering algorithm:

$\{\begin{matrix} {x x}_{k k} = = 0.5 0.5 {x x}_{k k - - 11} + + \frac{0.2 0.2 {x x}_{k k - - 11}}{11 + + {x x}_{k k - - 11}^{22}} + + {ω ω}_{k k - - 11} \\ {z z}_{k k} = = 1010 {x x}_{k k} + + \frac{{x x}_{k k}^{22}}{2020} + + {v v}_{k k} \end{matrix} - - - - - - ((5050))$

其中w_k和v_k均为高斯白噪声，且它们的常值统计特性如下： Among them, w _k and v _k are both Gaussian white noise, and their constant statistical properties are as follows:

$\{\begin{matrix} q q = = 1.2 1.2 \\ Q Q = = 0.6 0.6 \end{matrix} - - - - - - ((5151))$

$\{\begin{matrix} r r = = 1.0 1.0 \\ R R = = 0.8 0.8 \end{matrix} - - - - - - ((5252))$

设非线性系统和的初始值为： Let the initial value of the nonlinear system sum be:

$\{\begin{matrix} {\overset{^^}{x x}}_{00} = = 1.1 1.1 \\ {P P}_{00} = = 0.01 0.01 \end{matrix} - - - - - - ((5353))$

且与w_k和v_k是互不相关的。 and and w _k and v _k are independent of each other.

考虑上面提出的强非线性高斯系统模型，假设量测噪声统计特性精确已知(如式)，而过程噪声统计特性未知或不准确，且取初始均值和协方差为： Consider the strong nonlinear Gaussian system model proposed above, assuming that the statistical properties of the measurement noise are known precisely (such as Equation), while the statistical properties of the process noise are unknown or inaccurate, and the initial mean and covariance are taken as:

${\overset{^^}{q q}}_{00} = = 0.2 0.2,, {\overset{^^}{Q Q}}_{00} = = 0.5 0.5$

为了验证改进后的自适应UKF滤波算法的优越性，在这里分别采用了文献[7]提出的UKF算法和本发明提出的自适应UKF算法对Q进行自适应估计仿真，仿真结果如下： In order to verify the superiority of the improved adaptive UKF filtering algorithm, the UKF algorithm proposed in the literature [7] and the adaptive UKF algorithm proposed by the present invention are used to perform adaptive estimation simulation on Q. The simulation results are as follows:

仍考虑上面的强非线性高斯系统模型，假设此时系统噪声w和v均为高斯噪声且服从正态分布，非线性系统的过程噪声精确已知(如式)而量测噪声统计特性未知或不准确，且取初始值均值和协方差为： Still consider the above strong nonlinear Gaussian system model, assuming that the system noise w and v are both Gaussian noise and obey the normal distribution, the process noise of the nonlinear system is precisely known (such as formula) and the statistical characteristics of the measurement noise are unknown or Inaccurate, and take the initial value mean and covariance as:

${\overset{^^}{r r}}_{00} = = 0.3 0.3,, {\overset{^^}{R R}}_{00} = = 0.6 0.6$

为了验证改进后的自适应UKF滤波算法的优越性，在这里同样分别采用了文献[7]提出的UKF算法和本发明提出的自适应UKF算法对R进行自适应估计仿真 In order to verify the superiority of the improved adaptive UKF filtering algorithm, the UKF algorithm proposed by the literature [7] and the adaptive UKF algorithm proposed by the present invention are also used here to perform adaptive estimation simulation on R

以上所述，仅为本发明最佳实施方式，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。 The above is only the best implementation mode of the present invention, any simple changes or equivalent replacements of the technical solutions that can be clearly obtained by any person skilled in the art within the technical scope disclosed in the present invention all fall into the scope of the present invention within the scope of protection.

参考文献 references

[1]卢舒勃,杨永胜,敬忠良.基于UKF的INS/GPS组合导航系统仿真[J].科学技术与工程,2011,04:773-778. [1] Lu Shubo, Yang Yongsheng, Jing Zhongliang. Simulation of INS/GPS integrated navigation system based on UKF [J]. Science Technology and Engineering, 2011, 04:773-778.

[2]田海,朱新岩.一种简化的SAGE-HUSA卡尔曼滤波[J].弹箭与制导学报,2011,01:75-77+84. [2] Tian Hai, Zhu Xinyan. A Simplified SAGE-HUSA Kalman Filter[J]. Journal of Bullets, Arrows and Guidance, 2011, 01:75-77+84.

[3]石勇,韩崇昭.自适应UKF算法在目标跟踪中的应用[J].自动化学报,2011,06:755-759. [3] Shi Yong, Han Chongzhao. Application of Adaptive UKF Algorithm in Target Tracking [J]. Acta Automatica Sinica, 2011, 06:755-759.

[4]岳晓奎,袁建平.一种基于极大似然准则的自适应卡尔曼滤波算法[J].西北工业大学学报,2005,04:469-474. [4] Yue Xiaokui, Yuan Jianping. An Adaptive Kalman Filtering Algorithm Based on Maximum Likelihood Criterion [J]. Journal of Northwestern Polytechnical University, 2005, 04:469-474.

[5]王璐,李光春,乔相伟,王兆龙,马涛.基于极大似然准则和最大期望算法的自适应UKF算法[J].自动化学报,2012,07:1200-1210. [5] Wang Lu, Li Guangchun, Qiao Xiangwei, Wang Zhaolong, Ma Tao. Adaptive UKF algorithm based on maximum likelihood criterion and maximum expectation algorithm [J]. Acta Automatica Sinica, 2012, 07:1200-1210.

[6]冯琼华,吴铁军,马龙华.高动态下基于AUKF的载波跟踪算法[J].计算机工程,2012,16:237-240+244. [6] Feng Qionghua, Wu Tiejun, Ma Longhua. Carrier Tracking Algorithm Based on AUKF under High Dynamics [J]. Computer Engineering, 2012,16:237-240+244.

[7]赵琳,王小旭,孙明,丁继成,闫超.基于极大后验估计和指数加权的自适应UKF滤波算法[J].自动化学报,2010,07:1007-1019. [7] Zhao Lin, Wang Xiaoxu, Sun Ming, Ding Jicheng, Yan Chao. Adaptive UKF filter algorithm based on maximum a posteriori estimation and exponential weighting [J]. Acta Automatica Sinica, 2010, 07:1007-1019.

[8]邓江安,孔祥维,孙庆鹏,卢聪聪.用一种自适应UKF滤波器跟踪机动目标[J].火力与指挥控制,2008,S1:56-59. [8] Deng Jiangan, Kong Xiangwei, Sun Qingpeng, Lu Congcong. Using an Adaptive UKF Filter to Track Maneuvering Targets [J]. Firepower and Command and Control, 2008, S1:56-59.

[9]高为广,何海波,陈金平.自适应UKF算法及其在GPS/INS组合导航中的应用[J].北京理工大学学报,2008,06:505-509. [9] Gao Weiguang, He Haibo, Chen Jinping. Adaptive UKF algorithm and its application in GPS/INS integrated navigation [J]. Journal of Beijing Institute of Technology, 2008, 06:505-509.

[10]胡高歌,高社生,赵岩.一种新的自适应UKF算法及其在组合导航中的应用[J].中国惯性技术学报,2014,03:357-361+367. [10] Hu Gaoge, Gao Shesheng, Zhao Yan. A new adaptive UKF algorithm and its application in integrated navigation [J]. Chinese Journal of Inertial Technology, 2014, 03:357-361+367.

[11]刘铮.UKF算法及其改进算法的研究[D].中南大学,2009。 [11] Liu Zheng. Research on UKF Algorithm and Its Improved Algorithm [D]. Central South University, 2009.

Claims

1. An adaptive UKF filtering algorithm, characterized by comprising the steps of: the first step is as follows: judging whether convergence has been stabilized

Q_k-Q_k-2≤H

R_k-R_k-2≤H

Wherein H is a small positive integer;

the second step is that: selecting a figure of merit

Let the estimated covariance of the two algorithms be Q1_k、R1_k、Q2_k、R2_kThen, then

{\hat{Q}}_{k} = \frac{{Q 1}_{k} - {Q 2}_{k}}{2}

{\hat{R}}_{k} = \frac{{R 1}_{k} + {R 2}_{k}}{2}

In the formula,is the covariance estimate.