CN111969979A - Minimum error entropy CDKF filter method - Google Patents

Abstract

The invention proposes a minimum error entropy CDKF filter method, which belongs to the technical field of robot navigation and positioning and is used to solve the technical problem of computational instability of the central difference filter algorithm. The present invention introduces a new minimum error entropy criterion in the observation update step, and jointly implements the system model expansion operation through the nonlinear system prediction noise error and the observation noise to obtain a new system noise expression. According to the second-order information potential energy based on the Renyis entropy criterion The formula constructs the error cost function, thereby designing the observation update calculation process of the CDKF algorithm, thus constructing a new central difference filtering algorithm calculation framework based on error entropy. This invention effectively improves the calculation instability problem of the traditional CDKF algorithm through the second-order minimum error entropy information potential differential calculation of the observation update step. Through simulation verification of the motion state of the land-based robot, the calculation efficiency of the MEE-CDKF algorithm is improved, and the calculation Accuracy is guaranteed.

Description

A Minimum Error Entropy CDKF Filter Method

technical field

The invention relates to the technical field of robot navigation and positioning, in particular to a minimum error entropy CDKF filter method.

Background technique

The traditional central difference filtering method is a kind of suboptimal filtering algorithm obtained by using Stiring interpolation approximation numerical calculation method under the framework of Bayesian optimal filtering theory. The volume Kalman filtering algorithm also uses different numerical approximation calculation methods to obtain the optimal or sub-optimal iterative filtering calculation method of the state variable parameters of the state space model of the nonlinear system, which all belong to the point approximation optimal estimated value in the sense of probability distribution. processing algorithm. They have a common feature that based on the characteristics of Gaussian noise distribution, the popular Minimum Mean Square Error (MMSE) is used to carry out the iterative filtering calculation process, but the MMSE criterion is not a good method for complex noise scenarios. Selection, especially for non-Gaussian noise scenarios, the above algorithms suffer from computational performance degradation.

In recent years, for heavy-tailed or impulsive non-Gaussian noise problems, some non-MMSE criteria have been applied to the robust Kalman filter algorithm to avoid the performance degradation of the filter algorithm. Among them, the Maximum Correntropy Criterion (MCC) in information learning theory It is applied to the design of Kalman filter algorithm to deal with impulse noise interference, thus constructing the maximum coentropy Kalman filter algorithm, the maximum coentropy extended Kalman algorithm, the maximum coentropy unscented Kalman algorithm and the maximum coentropy square root volume Kalman algorithm, etc. The advantage is that the maximum coentropy criterion is a local similarity measurement criterion, which is not sensitive to large errors, so the computational performance of the Kalman algorithm designed by the maximum coentropy will not be affected by relatively large external data. There is also a Minimum Error Entropy (MEE) criterion in information learning theory, and it has also gained important applications in robust convergence analysis, classification learning, system identification and adaptive filtering algorithms, and verified by numerical analysis, MEE criterion in The computational performance of many applications is much higher than that of the MCC criterion. For example, in terms of computational complexity, the MEE criterion has significantly lower computational complexity than the MCC criterion.

SUMMARY OF THE INVENTION

Aiming at the technical problem that the traditional CDKF algorithm is unstable in calculation, the present invention proposes a minimum error entropy CDKF filter method, which uses the minimum error entropy criterion and designs a second-order minimum error entropy central difference filtering algorithm based on the second-order information potential energy. Prediction error and observation error Design an extended error model system, calculate the state variance of the extended error system and its square root variance matrix, realize the model transformation calculation of error noise, construct the second-order minimum error entropy information potential energy calculation expression formula, and expand the partial differential calculation to obtain it The optimal estimated value of the system state variable, and then calculate its optimal estimated variance matrix.

The technical scheme of the present invention is realized as follows:

A minimum error entropy CDKF filter method, the steps are as follows:

Step 1: Construct the state space model of the nonlinear discrete system of the land-based robot, and use the second-order Stirling interpolation polynomial to perform numerical integral approximation calculation on the nonlinear discrete system, and obtain the deterministic sampling points and linear expressions of the nonlinear discrete system;

估计误差方差矩阵P_k-1，获得k-1时刻的状态变量估计值的确定性采样点和加权系数；Step 2. According to the estimated value of the state variable of the state space model of the nonlinear discrete system at time k-1

Estimating the error variance matrix P _k-1 to obtain the deterministic sampling points and weighting coefficients of the estimated value of the state variable at time k-1;

Step 3: Determine the weighted sampling point set according to the deterministic sampling points of the estimated value of the state variable at time k-1, and predict the predicted value of the state variable of the nonlinear discrete system at time k

Step 4: Obtain the prediction error of the state variable of the nonlinear discrete system according to the predicted value of the state variable of the nonlinear discrete system, and expand and organize the state space model of the nonlinear discrete system according to the prediction error to obtain the expanded noise term of the nonlinear discrete system;

Step 5: Calculate the extended noise error according to the extended noise term of the nonlinear discrete system, construct the minimum error entropy cost function of the extended noise error based on the second-order information potential energy according to the Renyis entropy, and obtain the nonlinear discrete system by minimizing the minimum error entropy cost function. Optimal value of state variable

估计方差矩阵和估计协方差矩阵；Step 6: Calculate the partial differential equation of the minimum error entropy cost function, and use the inverse matrix calculation principle to obtain the estimated value of the optimal value of the state variable according to the partial differential equation

estimated variance matrix and estimated covariance matrix;

设置参数τ，令

判断

若是，输出状态变量最优值的估计值

估计方差矩阵和估计协方差矩阵，执行步骤八，否则，返回步骤六，其中，

表示第k时刻系统状态变量上一步迭代估计值；Step 7. According to the estimated value of the optimal value of the state variable obtained in step 6

Set the parameter τ, let

judge

If so, output the estimated value of the optimal value of the state variable

Estimate the variance matrix and the estimated covariance matrix, and perform step 8, otherwise, go back to step 6, where,

Represents the estimated value of the previous iteration of the system state variable at the kth time;

Step 8: Calculate the posterior variance matrix of the state variable of the nonlinear discrete system according to the estimated variance matrix of the optimal value of the state variable.

The nonlinear discrete system state space model is:

Among them, x _k represents the system state variable at the kth time, x _k-1 represents the system state variable at the k-1th time, f( ) represents the system process function, h( ) represents the observation equation function, f( ) and h(·) are both nonlinear second-order differentiable functions, q _k-1 ∈ R ⁿ represents the time-varying process noise, and r _k ∈ R ^m represents the time-varying observation noise.

采样点是由0、he_i、-he_i(1≤i≤n)、he_i+he_j(1≤i≤j≤n)组成，参数h是插值步长，按照概率高斯分布特点，

则非线性离散系统状态空间模型可转化为线性表达式，The method of using the second-order Stirling interpolation polynomial to carry out the numerical integral approximation calculation on the nonlinear discrete system to obtain the deterministic sampling points and linear expressions of the nonlinear discrete system is: at the deterministic sampling point χ _i ,

The sampling point is composed of 0, he _i , -he _i (1≤i≤n), he _i +he _j (1≤i≤j≤n), and the parameter h is the interpolation step size. According to the characteristics of probability Gaussian distribution,

Then the nonlinear discrete system state space model can be transformed into a linear expression,

表示实施状态变量解耦后的系统过程函数在第0积分点的函数映射；Among them, s _i is the unit vector of the i-th coordinate axis of the integration point s∈R ^m , a∈R ^m denotes a vector, H=(H _ij ) _n×n is a symmetric matrix, n denotes the dimension of the system state variable,

Represents the function map of the system process function after decoupling of state variables at the 0th integration point;

The expression of the vector a∈R ^m and the symmetric matrix H=(H _ij ) _n×n is:

Among them, e _i represents the unit vector along the i-th axis, and e _j represents the unit vector along the j-th axis.

The method for obtaining the deterministic sampling points and the weighting coefficients of the estimated value of the state variable at the time k-1 is:

The estimated error variance matrix of the state-space model of the nonlinear discrete system at time k-1 is decomposed by Cholesky decomposition, and the square root of the estimated error variance matrix at time k-1 is obtained:

Among them, S _{x, k-1} represents the square root of the estimated error variance matrix at time k-1, and P _k-1 represents the estimated error variance matrix at time k-1;

The second-order Stirling interpolation polynomial is used to approximate the estimated value of the state variable of the state space model of the nonlinear discrete system at time k-1 and the square root of the estimated error variance matrix, and obtain the deterministic sampling point of the estimated value of the state variable at time k-1:

表示k-1时刻非线性离散系统状态空间模型的状态变量估计值；Among them, χ _0,k-1 represents the center sampling point determined according to the estimated value of the system state variable at the k-1th time, and χ _i,k-1 represents the division center determined according to the estimated value of the system state variable at the k-1th time the rest of the sampling points outside the sampling point,

represents the estimated value of the state variable of the state-space model of the nonlinear discrete system at time k-1;

Determine the weighting coefficient of the deterministic sampling point of the estimated value of the state variable at time k-1 according to the interpolation step h of the second-order Stirling interpolation polynomial:

表示确定性中心采样点的加权均值系数，

表示第i个确定性采样点的加权均值系数，

表示第i个确定性采样点的加权协方差系数。in,

represents the weighted mean coefficient of the deterministic center sampling point,

represents the weighted mean coefficient of the i-th deterministic sampling point,

Represents the weighted covariance coefficient of the ith deterministic sample point.

为：The predicted value of the state variable of the nonlinear discrete system at time k

for:

Wherein, χ _i,k,k-1 =f(χ _i,k-1 ) represents the weighted prediction value of the i-th sampling point at the k-th time point.

The prediction error of the state variable of the nonlinear discrete system is:

表示系统状态变量的预测误差；in,

represents the prediction error of the system state variable;

The method for expanding and sorting out the state space model of the nonlinear discrete system according to the prediction error to obtain the expanded noise term of the nonlinear discrete system is:

The state-space model of nonlinear discrete systems can be extended to obtain,

Among them, In represents the _n -dimensional identity matrix, and H _k represents the first-order Jacobian matrix of the observation function;

The extended noise term is defined as

The method for calculating the extended noise error according to the extended noise term of the nonlinear discrete system is:

Calculate the variance matrix of the extended noise term μ _k as,

的Cholesky分解算子矩阵，Θ_p,k,k-1表示P_k,k-1的Cholesky分解算子矩阵，Θ_r,k表示R_k的Cholesky分解算子矩阵；Among them, Θ _k represents

The Cholesky decomposition operator matrix of , Θ _{p, k, k-1} represents the Cholesky decomposition operator matrix of P _{k, k-1} , Θ _{r, k} represents the Cholesky decomposition operator matrix of R _k ;

整理获得，Multiply both sides of the extended model of the nonlinear discrete system state-space model by

get sorted,

d _k =W _k x _k +e _k ,

并且d_k＝(d_1,k,d_2,k,…,d_L,k)^T，W_k＝(w_1,k,w_2,k,…,w_L,k)^T，e_k＝(e_1,k,e_2,k,…,e_L,k)^T为扩展噪声误差，且L＝n+m。in,

and d _k =(d _1,k ,d _2,k ,...,d _L,k ) ^T , W _k =(w _1,k ,w _2,k ,...,w _L,k ) ^T ,e _k = (e _1,k ,e _2,k ,...,e _L,k ) ^T is the spread noise error, and L=n+m.

The minimum error entropy cost function of constructing the extended noise error based on the second-order information potential energy according to the Renyis entropy is:

Among them, G _σ represents the Gaussian kernel basis function, J _L (x _k ) represents the minimum error entropy cost function, i ₁ =1,2,...,L, j ₁ =1,2,...,L;

为：Optimal Values of State Variables for Nonlinear Discrete Systems

for:

The partial differential equation of the minimum error entropy cost function is:

in,

为：The estimated value of the optimal value of the state variable

for:

Λ_y,k表示根据第k时刻的观测向量获得的最小代价函数转换矩阵；in,

Λ _y,k represents the minimum cost function transformation matrix obtained according to the observation vector at the kth moment;

其中，

表示状态变量最优值的估计方差矩阵，Λ_x,k表示根据第k时刻的状态向量获得的最小代价函数转换矩阵；The estimated variance matrix is:

in,

Represents the estimated variance matrix of the optimal value of the state variable, Λ _x,k represents the minimum cost function transformation matrix obtained according to the state vector at the kth moment;

其中，

和

表示状态变量与观测向量间转换后的协方差矩阵，Λ_yx,k和Λ_xy,k表示状态变量与观测向量间转换矩阵。The estimated covariance matrix is:

in,

and

represents the transformed covariance matrix between the state variable and the observation vector, and Λ _yx,k and Λ _xy,k represent the transformation matrix between the state variable and the observation vector.

The posterior variance matrix of the state variables of the nonlinear discrete system is:

where P _k represents the posterior variance matrix of the state variables.

The beneficial effects that this technical solution can produce: the present invention effectively improves the computational instability problem of the traditional CDKF algorithm by observing the second-order minimum error entropy information potential differential calculation in the update step. The computational efficiency of the CDKF algorithm is improved, and the computational accuracy is guaranteed.

Description of drawings

In order to explain the embodiments of the present invention or the technical solutions in the prior art more clearly, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description are only These are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can also be obtained according to these drawings without creative efforts.

FIG. 1 is a calculation flow chart of the present invention.

FIG. 2 is a schematic diagram of the motion model of the mobile robot according to the method of the present invention.

FIG. 3 is a graph of the calculation error data of the mobile robot carrier of the method MEE-CDKF of the present invention.

Fig. 4 is the calculation diagram of the mobile robot carrier trajectory of the method MEE-CDKF of the present invention.

Figure 5 is a graph of the calculation error data of the mobile robot carrier obtained by the EKF algorithm.

Figure 6 is the calculation diagram of the trajectory of the mobile robot carrier obtained by the EKF algorithm.

Figure 7 is a graph of the calculation error data of the mobile robot carrier obtained by the SRUKF algorithm.

Figure 8 is the calculation diagram of the trajectory of the mobile robot carrier obtained by the SRUKF algorithm.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

Preliminary knowledge

The so-called minimum error entropy is different from the traditional MMSE or MCC criterion. Its goal is to minimize the information containing the error. Assuming that the error information is e, its definition adopts the concept of Renyis entropy.

Among them, α represents the order of Renyis entropy, which requires α>0, α≠1, and V _α (e) represents the information potential energy, which is defined as,

V _α (e)=∫p ^α (x)dx=E[p ^α-1 (e)] (2)

Among them, p(e) represents the probability density function of the error e, and E[p ^α-1 (e)] represents the expectation operator. In the actual central difference filtering algorithm design, the probability density function can be estimated and calculated by the Parzen window,

是N个误差采样数据序列。若取Renyi熵的阶次α＝2，那么二阶信息势能V₂(e)为，Here G _σ (xe _i ) is the Gaussian kernel basis function as,

is the N error sampled data sequence. If the order of Renyi entropy is taken as α=2, then the second-order information potential energy V ₂ (e) is,

Since the negative logarithmic function is monotonically decreasing, minimizing the error entropy H ₂ (e), this formula means maximizing the information potential energy

According to the calculation framework of the central difference filtering algorithm, if the prediction error in the state prediction update of the CDKF algorithm is defined as,

表示状态预测值，若非线性系统状态空间模型方程表达式为，in

represents the state prediction value, if the nonlinear system state space model equation is expressed as,

处对过程函数实施线性化逼近计算，在

处对观测函数实施线性化逼近计算，获得，Implement Taylor series expansion approximation calculation, in

Perform a linearized approximation calculation on the process function at

Perform a linear approximation calculation on the observation function at , and obtain,

对式(5)进行变换，结合式(7)，在此基础上对系统模型进行扩展，可以获得，here,

Transforming formula (5), combining formula (7), and extending the system model on this basis, we can obtain,

它融合了状态预测误差和观测误差信息，那么其方差矩阵可计算为，And the extended noise term in Eq. (8) is defined as

It fuses state prediction error and observation error information, then its variance matrix can be calculated as,

P_k,k-1和R_k的Cholesky分解算子矩阵，将前面式子(8)两边都乘以

整理获得，Here Θ _k , Θ _p,k,k-1 and Θ _r,k represent respectively

For the Cholesky decomposition operator matrix of P _{k, k-1} and R _k , multiply both sides of the previous equation (8) by

get sorted,

d _k =W _k x _k +e _k (10)

并且d_k＝(d_1,k,d_2,k,…,d_L,k)^T，W_k＝(w_1,k,w_2,k,…,w_L,k)^T，e_k＝(e_1,k,e_2,k,…,e_L,k)^T，且L＝n+m。in

and d _k =(d _1,k ,d _2,k ,...,d _L,k ) ^T , W _k =(w _1,k ,w _2,k ,...,w _L,k ) ^T ,e _k = (e _1,k ,e _2,k ,...,e _L,k ) ^T , and L=n+m.

Based on the second-order information potential (4), define the cost function,

Then, the estimated value of the state variable of the nonlinear system can be obtained by minimizing the cost function (11).

Thus, the partial derivative of the cost function with respect to x _k is calculated,

由此可利用固定点迭代计算系统状态变量估计，Here (Ψ _k ) _ij = G _σ ( _ei,k -e _j,k ),

From this, the system state variable estimates can be calculated iteratively using fixed point,

且满足Λ_k∈R^L×L，Λ_x,k∈R^n×n，Λ_xy,k∈R^m×n，Λ_yx,k∈R^n×m，Λ_y,k∈R^m×m，从而可以获得，here

and satisfy Λ _k ∈R ^L×L , Λ _x,k ∈R ^n×n , Λ _xy,k ∈R ^m×n , Λ _yx,k ∈R ^n×m , Λ _y,k ∈R ^m×m , so that it can be obtained,

Therefore, the above two formulas (15) and (16) can be sorted and standardized as:

According to equations (15) and (16), the matrix Π ₁ Π ₂ Π ₃ is defined as,

According to the matrix inverse calculation theorem, the iterative recursive calculation formula for the estimation of the system state variables is obtained by arranging the formula (17) as,

here

The corresponding system state variable posterior variance matrix can be calculated as,

Aiming at the computational instability of the central difference filtering algorithm, the invention revises the filtering calculation criterion from the minimum mean square error MMSE to the minimum error entropy criterion MEE, thereby designing and obtaining a novel minimum error entropy CDKF filtering algorithm. The present invention is based on the traditional central difference filtering algorithm based on the minimum mean square error criterion, and is oriented to the nonlinear system state space model. The minimum error entropy criterion is used to obtain a new system noise expression by jointly implementing the system model expansion operation with the prediction noise error of the nonlinear system and the observation noise. The error cost function is constructed according to the second-order information potential formula based on the Renyis entropy criterion, and the CDKF is designed. According to the observation update calculation process of the algorithm, a new calculation framework of the central difference filtering algorithm based on error entropy is constructed. Using the method of the invention to carry out the simulation verification of the positioning calculation of the land-based robot, the calculation accuracy of the method of the invention is improved, and the calculation stability is obviously improved and improved compared with the traditional CDKF algorithm.

The minimum error entropy criterion is a method for calculating the minimum value of error information based on the Renyis entropy formula. It introduces the concept of information potential energy in machine learning theory, and abstracts the second-order information potential energy expression to describe the prediction error of the nonlinear system state variables in the CDKF algorithm. , together with the observation equation of the nonlinear system state space model, obtain the nonlinear system extended error state model, calculate the noise variance of the extended error state model, and perform Cholesky decomposition on it to obtain the square root variance positive definite matrix of the extended noise, using the square root noise variance matrix The extended error state model is transformed and sorted to obtain the joint error expression of prediction error and observation noise, and then the CDKF prediction error cost function is synthesized by the second-order information potential energy expression of the minimum error entropy criterion, then the minimum error entropy cost function is minimized to get To obtain the optimal calculation of the system state variables, the specific method is to calculate the partial differential of the minimization error entropy cost function and set it to 0. At the same time, the matrix inverse theorem is used to realize the optimal calculation of the system state variables. The explicit expression, and in the computer A small number of parameters are set in the algorithm preparation to judge the estimated value of the system state variable calculated by each iteration. If the judgment expression is established, the calculation of the variance matrix of the estimated system state variable is continued; otherwise, the calculation process of the previous steps is continued, and finally Obtain the computation of the estimated variance matrix of the system state variables.

As shown in FIG. 1, an embodiment of the present invention provides a minimum error entropy CDKF filter method, and the specific steps are as follows:

Step 1: Construct a state space model of the nonlinear discrete system of the land-based robot, and use the second-order Stirling interpolation polynomial to perform a numerical integral approximation calculation on the nonlinear discrete system to obtain deterministic sampling points and linear expressions of the nonlinear discrete system; The state space model of the nonlinear discrete system is:

Among them, x _k represents the system state variable at the kth time, x _k-1 represents the system state variable at the k-1th time, f( ) represents the system process function, h( ) represents the observation equation function, f( ) and h(·) are both nonlinear second-order differentiable functions, q _k-1 ∈ R ⁿ represents the time-varying process noise, and r _k ∈ R ^m represents the time-varying observation noise.

In the design process of the nonlinear system state space model, the linearization approximation calculation is performed for the nonlinear system function of the moving target, assuming that any nonlinear function is,

y=f(x) (23),

Implementing the Gauss-Hermite quadrature formula approximation is calculated as,

采样点是由0、he_i、-he_i(1≤i≤n)、he_i+he_j(1≤i≤j≤n)组成，参数h是插值步长，按照概率高斯分布特点，

则非线性离散系统状态空间模型可转化为线性表达式，It can use 2m-1 order polynomial to approximate nonlinear functions arbitrarily, and the central difference filtering algorithm uses the second-order Stirling interpolation polynomial method to realize. At the deterministic sampling point χ _i ,

The sampling point is composed of 0, he _i , -he _i (1≤i≤n), he _i +he _j (1≤i≤j≤n), and the parameter h is the interpolation step size. According to the characteristics of probability Gaussian distribution,

Then the nonlinear discrete system state space model can be transformed into a linear expression,

表示实施状态变量解耦后的系统过程函数在第0积分点的函数映射；Among them, s _i is the unit vector of the i-th coordinate axis of the integration point s∈R ^m , a∈R ^m denotes a vector, H=(H _ij ) _n×n is a symmetric matrix, n denotes the dimension of the system state variable,

Represents the function map of the system process function after decoupling of state variables at the 0th integration point;

The expression of the vector a∈R ^m and the symmetric matrix H=(H _ij ) _n×n is:

Among them, e _i represents the unit vector along the i-th axis, and e _j represents the unit vector along the j-th axis; thus, the numerical integral approximation calculation of the nonlinear function can be realized.

估计误差方差矩阵P_k-1，获得k-1时刻的状态变量估计值的确定性采样点和加权系数；Step 2. According to the estimated value of the state variable of the state space model of the nonlinear discrete system at time k-1

Estimating the error variance matrix P _k-1 to obtain the deterministic sampling points and weighting coefficients of the estimated value of the state variable at time k-1;

The estimated error variance matrix of the state-space model of the nonlinear discrete system at time k-1 is decomposed by Cholesky decomposition, and the square root of the estimated error variance matrix at time k-1 is obtained:

Among them, S _{x, k-1} represents the square root of the estimated error variance matrix at time k-1, and P _k-1 represents the estimated error variance matrix at time k-1;

The second-order Stirling interpolation polynomial is used to approximate the estimated value of the state variable of the state space model of the nonlinear discrete system at time k-1 and the square root of the estimated error variance matrix, and obtain the deterministic sampling point of the estimated value of the state variable at time k-1:

表示k-1时刻非线性离散系统状态空间模型的状态变量估计值；Among them, χ _0,k-1 represents the center sampling point determined according to the estimated value of the system state variable at the k-1th time, and χ _i,k-1 represents the division center determined according to the estimated value of the system state variable at the k-1th time the rest of the sampling points outside the sampling point,

represents the estimated value of the state variable of the state-space model of the nonlinear discrete system at time k-1;

Determine the weighting coefficient of the deterministic sampling point of the estimated value of the state variable at time k-1 according to the interpolation step h of the second-order Stirling interpolation polynomial:

表示确定性中心采样点的加权均值系数，

表示第i个确定性采样点的加权均值系数，

表示第i个确定性采样点的加权协方差系数。in,

represents the weighted mean coefficient of the deterministic center sampling point,

represents the weighted mean coefficient of the i-th deterministic sampling point,

Represents the weighted covariance coefficient of the ith deterministic sample point.

Step 3: Determine the weighted sampling point set according to the deterministic sampling points of the estimated value of the state variable at time k-1, and predict the predicted value of the state variable of the nonlinear discrete system at time k

为：The predicted value of the state variable of the nonlinear discrete system at time k

for:

Wherein, χ _i,k,k-1 =f(χ _i,k-1 ) represents the weighted prediction value of the i-th sampling point at the k-th time point.

Step 4: Obtain the prediction error of the state variable of the nonlinear discrete system according to the predicted value of the state variable of the nonlinear discrete system, and expand and organize the state space model of the nonlinear discrete system according to the prediction error to obtain the expanded noise term of the nonlinear discrete system;

The prediction error of the state variable of the nonlinear discrete system is:

表示状态变量预测误差；in,

represents the state variable prediction error;

The method for expanding and sorting out the state space model of the nonlinear discrete system according to the prediction error to obtain the expanded noise term of the nonlinear discrete system is:

The state-space model of nonlinear discrete systems can be extended to obtain,

Among them, In represents the _n -dimensional identity matrix, and H _k represents the first-order Jacobian matrix of the observation function;

扩展噪声项融合了状态预测误差和观测误差信息。The extended noise term is defined as

The extended noise term fuses state prediction error and observation error information.

Step 5: Calculate the extended noise error according to the extended noise term of the nonlinear discrete system, construct the minimum error entropy cost function of the extended noise error based on the second-order information potential energy according to the Renyis entropy, and obtain the nonlinear discrete system by minimizing the minimum error entropy cost function. Optimal value of state variable

Calculate the variance matrix of the extended noise term μ _k as,

的Cholesky分解算子矩阵，Θ_p,k,k-1表示P_k,k-1的Cholesky分解算子矩阵，Θ_r,k表示R_k的Cholesky分解算子矩阵；Among them, Θ _k represents

The Cholesky decomposition operator matrix of , Θ _{p, k, k-1} represents the Cholesky decomposition operator matrix of P _{k, k-1} , Θ _{r, k} represents the Cholesky decomposition operator matrix of R _k ;

整理获得，Multiply both sides of the extended model of the nonlinear discrete system state-space model by

get sorted,

d _k =W _k x _k +e _k (34),

并且d_k＝(d_1,k,d_2,k,…,d_L,k)^T，W_k＝(w_1,k,w_2,k,…,w_L,k)^T，e_k＝(e_1,k,e_2,k,…,e_L,k)^T为扩展噪声误差，且L＝n+m。in,

and d _k =(d _1,k ,d _2,k ,...,d _L,k ) ^T , W _k =(w _1,k ,w _2,k ,...,w _L,k ) ^T ,e _k = (e _1,k ,e _2,k ,...,e _L,k ) ^T is the spread noise error, and L=n+m.

The minimum error entropy cost function of constructing the extended noise error based on the second-order information potential energy according to the Renyis entropy is:

Among them, G _σ represents the Gaussian kernel basis function, J _L (x _k ) represents the minimum error entropy cost function, i ₁ =1,2,...,L, j ₁ =1,2,...,L;

为：Optimal Values of State Variables for Nonlinear Discrete Systems

for:

估计方差矩阵和估计协方差矩阵；Step 6: Calculate the partial differential equation of the minimum error entropy cost function, and use the inverse matrix calculation principle to obtain the estimated value of the optimal value of the state variable according to the partial differential equation

estimated variance matrix and estimated covariance matrix;

The partial differential equation of the minimum error entropy cost function is:

in,

According to the partial differential equation, the system state variable estimates are calculated using fixed point iterations,

且满足Λ_k∈R^L×L，Λ_x,k∈R^n×n，Λ_xy,k∈R^m×n，Λ_yx,k∈R^n×m，Λ_y,k∈R^m×m，从而可以获得，in,

and satisfy Λ _k ∈R ^L×L , Λ _x,k ∈R ^n×n , Λ _xy,k ∈R ^m×n , Λ _yx,k ∈R ^n×m , Λ _y,k ∈R ^m×m , so that it can be obtained,

Therefore, the above two formulas (39) and (40) are sorted and standardized as,

According to equations (39) and (40), the matrix Π ₁ Π ₂ Π ₃ is defined as,

为：Using the matrix inverse calculation theorem, the estimated value of the optimal value of the state variable is obtained by arranging the formula (41).

for:

Λ_y,k表示根据第k时刻的观测向量获得的最小代价函数转换矩阵；in,

Λ _y,k represents the minimum cost function transformation matrix obtained according to the observation vector at the kth moment;

其中，

表示状态变量最优值的估计方差矩阵，Λ_x,k表示根据第k时刻的状态向量获得的最小代价函数转换矩阵；The estimated variance matrix is:

in,

Represents the estimated variance matrix of the optimal value of the state variable, Λ _x,k represents the minimum cost function transformation matrix obtained according to the state vector at the kth moment;

其中，

和

表示状态变量与观测向量间转换后的协方差矩阵，Λ_yx,k和Λ_xy,k表示状态变量与观测向量间转换矩阵。The estimated covariance matrix is:

in,

and

represents the transformed covariance matrix between the state variable and the observation vector, and Λ _yx,k and Λ _xy,k represent the transformation matrix between the state variable and the observation vector.

设置参数τ，令

判断

若是，输出状态变量最优值的估计值

估计方差矩阵和估计协方差矩阵，执行步骤八，否则，返回步骤六，其中，

表示第k时刻系统状态变量上一步迭代估计值；Step 7. According to the estimated value of the optimal value of the state variable obtained in step 6

Set the parameter τ, let

judge

If so, output the estimated value of the optimal value of the state variable

Estimate the variance matrix and the estimated covariance matrix, and perform step 8, otherwise, go back to step 6, where,

Represents the estimated value of the previous iteration of the system state variable at the kth time;

Step 8: Calculate the posterior variance matrix of the state variable of the nonlinear discrete system according to the estimated variance matrix of the optimal value of the state variable.

The posterior variance matrix of the state variables of the nonlinear discrete system is:

where P _k represents the posterior variance matrix of the state variables.

Applications

In order to verify the computational efficiency of the minimum error entropy center differential filtering algorithm proposed by the present invention, the method of the present invention is used to carry out simulation verification and calculation on the model of the ground-based robot positioning system to prove the effectiveness of the method of the present invention and its computational advantages. The simulation is given here. Validate test data. Consider a ground mobile robot system, which adopts the front wheel drive mode, as shown in Figure 2, the front wheel steering angle is defined as α, the counterclockwise direction is the positive direction, and the steering of the robot coordinate system xy _Robot relative to the ground coordinate system xy _ground The angle is ψ, positive in the counterclockwise direction, and the rear wheel speed is defined as v _rearwheel . From the geometric figure, it can be seen that the distance R from the observation point to the center of the rear wheel and the wheelbase L of the front and rear wheels satisfy

同时我们可以得到后轮速度表达式为

可以整理获得机器人转向角方程为，Thus, the observation point distance can be obtained,

At the same time, we can get the rear wheel speed expression as

The steering angle equation of the robot can be sorted out as,

Arrange to get

If the motion speed of the mobile robot system is considered again, the dynamic equation of the mobile robot system can be obtained in the robot coordinate system as:

Convert the dynamic equation of the robot system to the ground coordinate system to obtain the final movement equation of the mobile robot system in the ground coordinate system

The parameter α(t) in the system equation can be used as the modulation parameter, as the input variable of the system, to generate a control parameter of the front wheel, which satisfies the control law

The parameter ψ _des here represents the desired heading angle, and the parameter ψ represents the current moving heading angle, then the desired heading angle can be expressed as

Generally speaking, the range of the steering angle is within (±π/4), and the G parameter affects the speed of the robot's turning. The purpose of the motion equation of the mobile robot system is to estimate the system state variables and track the motion path. Therefore, there are many sensors to perceive the coordinate position of the system, such as GPS or forward coding direction finder and other equipment to complete the target tracking and observation. The two-way position coordinates of the mobile robot in the coordinate system are used as observation variables, so the observation equation is linear and can be obtained directly. Consider the initial variance matrix of the system as

In addition, the system parameters are set as the wheelbase of the front and rear wheels of the mobile robot is L=2m, the control law gain is G=2, the moving speed is kept at v=1m/s, and the sampling time interval δt=0.1s; it is assumed that only the heading angle ψ is disturbed in the process variables. noise, the process noise variance can be set as Q=0.05 ² . The observation variable is the position coordinate of the mobile robot, so the observation noise variance matrix can be set as

Thereby, the simulation results of the movement trajectory of the mobile robot system can be obtained. Here, the calculation results of the EKF algorithm and the SRUKF algorithm are compared with the calculation results of the MEE-CDKF algorithm of the algorithm of the present invention, and the results are shown in Figures 3-8. Using the EKF algorithm and the SRUKF algorithm to compare the MCC-CDKF algorithm of the present invention, it can be seen that the MEE-CDKF algorithm has relatively good computational stability, fast computational convergence speed, and significantly improved computational accuracy.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention shall be included in the scope of the present invention. within the scope of protection.

Claims (10)

1. a minimum error entropy CDKF filter method, is characterized in that, its steps are as follows: Step 1: Construct the state space model of the nonlinear discrete system of the land-based robot, and use the second-order Stirling interpolation polynomial to perform numerical integral approximation calculation on the nonlinear discrete system, and obtain the deterministic sampling points and linear expressions of the nonlinear discrete system; Step 2. According to the estimated value of the state variable of the state space model of the nonlinear discrete system at time k-1

Estimating the error variance matrix P _k-1 to obtain the deterministic sampling points and weighting coefficients of the estimated value of the state variable at time k-1; Step 3: Determine the weighted sampling point set according to the deterministic sampling points of the estimated value of the state variable at time k-1, and predict the predicted value of the state variable of the nonlinear discrete system at time k

Step 4: Obtain the prediction error of the state variable of the nonlinear discrete system according to the predicted value of the state variable of the nonlinear discrete system, and expand and organize the state space model of the nonlinear discrete system according to the prediction error to obtain the expanded noise term of the nonlinear discrete system; Step 5: Calculate the extended noise error according to the extended noise term of the nonlinear discrete system, construct the minimum error entropy cost function of the extended noise error based on the second-order information potential energy according to the Renyis entropy, and obtain the nonlinear discrete system by minimizing the minimum error entropy cost function. Optimal value of state variable

Step 6: Calculate the partial differential equation of the minimum error entropy cost function, and use the inverse matrix calculation principle to obtain the estimated value of the optimal value of the state variable according to the partial differential equation

estimated variance matrix and estimated covariance matrix; Step 7. According to the estimated value of the optimal value of the state variable obtained in step 6

Set the parameter τ, let

judge

If so, output the estimated value of the optimal value of the state variable

Estimate the variance matrix and the estimated covariance matrix, and perform step 8, otherwise, go back to step 6, where,

Represents the estimated value of the previous iteration of the system state variable at the kth time; Step 8: Calculate the posterior variance matrix of the state variable of the nonlinear discrete system according to the estimated variance matrix of the optimal value of the state variable. 2. minimum error entropy CDKF filter method according to claim 1, is characterized in that, described nonlinear discrete system state space model is:

Among them, x _k represents the system state variable at the kth time, x _k-1 represents the system state variable at the k-1th time, f( ) represents the system process function, h( ) represents the observation equation function, f( ) and h(·) are both nonlinear second-order differentiable functions, q _k-1 ∈ R ⁿ represents the time-varying process noise, and r _k ∈ R ^m represents the time-varying observation noise. 3. minimum error entropy CDKF filter method according to claim 2, is characterized in that, described utilizes second-order Stirling interpolation polynomial to carry out numerical integral approximation calculation to nonlinear discrete system, obtains the deterministic sampling point of nonlinear discrete system And the method of linear expression is: at the deterministic sampling point χ _i ,

The sampling point is composed of 0, he _i , -he _i (1≤i≤n), he _i +he _j (1≤i≤j≤n), and the parameter h is the interpolation step size. According to the characteristics of probability Gaussian distribution,

Then the nonlinear discrete system state space model can be transformed into a linear expression,

Among them, s _i is the unit vector of the i-th coordinate axis of the integration point s∈R ^m , a∈R ^m denotes a vector, H=(H _ij ) _n×n is a symmetric matrix, n denotes the dimension of the system state variable,

Represents the function map of the system process function after decoupling of state variables at the 0th integration point; The expression of the vector a∈R ^m and the symmetric matrix H=(H _ij ) _n×n is:

Among them, e _i represents the unit vector along the i-th axis, and e _j represents the unit vector along the j-th axis. 4. minimum error entropy CDKF filter method according to claim 3, is characterized in that, the obtaining method of the deterministic sampling point of the state variable estimated value of described k-1 moment and weighting coefficient is: The estimated error variance matrix of the state-space model of the nonlinear discrete system at time k-1 is decomposed by Cholesky decomposition, and the square root of the estimated error variance matrix at time k-1 is obtained:

Among them, S _{x, k-1} represents the square root of the estimated error variance matrix at time k-1, and P _k-1 represents the estimated error variance matrix at time k-1; The second-order Stirling interpolation polynomial is used to approximate the estimated value of the state variable of the state space model of the nonlinear discrete system at time k-1 and the square root of the estimated error variance matrix, and obtain the deterministic sampling point of the estimated value of the state variable at time k-1:

Among them, χ _0,k-1 represents the center sampling point determined according to the estimated value of the system state variable at the k-1th time, and χ _i,k-1 represents the division center determined according to the estimated value of the system state variable at the k-1th time the rest of the sampling points outside the sampling point,

represents the estimated value of the state variable of the state-space model of the nonlinear discrete system at time k-1; Determine the weighting coefficient of the deterministic sampling point of the estimated value of the state variable at time k-1 according to the interpolation step h of the second-order Stirling interpolation polynomial:

in,

represents the weighted mean coefficient of the deterministic center sampling point,

represents the weighted mean coefficient of the i-th deterministic sampling point,

Represents the weighted covariance coefficient of the ith deterministic sample point. 5. The minimum error entropy CDKF filter method according to claim 4, wherein the predicted value of the state variable of the nonlinear discrete system at the k time

for:

Wherein, χ _i,k,k-1 =f(χ _i,k-1 ) represents the weighted prediction value of the i-th sampling point at the k-th time point. 6. minimum error entropy CDKF filter method according to claim 5, is characterized in that, the prediction error of the state variable of described nonlinear discrete system is:

in,

represents the prediction error of the system state variable; The method for expanding and sorting out the state space model of the nonlinear discrete system according to the prediction error to obtain the expanded noise term of the nonlinear discrete system is: The state-space model of nonlinear discrete systems can be extended to obtain,

Among them, In represents the _n -dimensional identity matrix, and H _k represents the first-order Jacobian matrix of the observation function; The extended noise term is defined as

7. The minimum error entropy CDKF filter method according to claim 6, wherein the method for calculating the extended noise error according to the extended noise term of the nonlinear discrete system is: Calculate the variance matrix of the extended noise term μ _k as,

Among them, Θ _k represents

The Cholesky decomposition operator matrix of , Θ _{p, k, k-1} represents the Cholesky decomposition operator matrix of P _{k, k-1} , Θ _{r, k} represents the Cholesky decomposition operator matrix of R _k ; Multiply both sides of the extended model of the nonlinear discrete system state-space model by

get sorted, d _k =W _k x _k +e _k , in,

and d _k =(d _1,k ,d _2,k ,...,d _L,k ) ^T , W _k =(w _1,k ,w _2,k ,...,w _L,k ) ^T ,e _k = (e _1,k ,e _2,k ,...,e _L,k ) ^T is the spread noise error, and L=n+m. 8. minimum error entropy CDKF filter method according to claim 7, is characterized in that, described according to Renyis entropy based on second-order information potential energy constructs the minimum error entropy cost function of extended noise error is:

Among them, G _σ represents the Gaussian kernel basis function, J _L (x _k ) represents the minimum error entropy cost function, i ₁ =1,2,...,L, j ₁ =1,2,...,L; Optimal Values of State Variables for Nonlinear Discrete Systems

for:

9. minimum error entropy CDKF filter method according to claim 8, is characterized in that, the partial differential equation of described minimum error entropy cost function is:

in,

The estimated value of the optimal value of the state variable

for:

in,

Λ _y,k represents the minimum cost function transformation matrix obtained according to the observation vector at the kth moment; The estimated variance matrix is:

in,

Represents the estimated variance matrix of the optimal value of the state variable, Λ _x,k represents the minimum cost function transformation matrix obtained according to the state vector at the kth moment; The estimated covariance matrix is:

in,

and

represents the transformed covariance matrix between the state variable and the observation vector, and Λ _yx,k and Λ _xy,k represent the transformation matrix between the state variable and the observation vector. 10. minimum error entropy CDKF filter method according to claim 9, is characterized in that, the posterior variance matrix of the state variable of described nonlinear discrete system is:

where P _k represents the posterior variance matrix of the state variables.

Images