CN111983926B - Maximum co-entropy extended ellipsoid collective filtering method - Google Patents

Maximum co-entropy extended ellipsoid collective filtering method Download PDF

Info

Publication number
CN111983926B
CN111983926B CN202010893973.8A CN202010893973A CN111983926B CN 111983926 B CN111983926 B CN 111983926B CN 202010893973 A CN202010893973 A CN 202010893973A CN 111983926 B CN111983926 B CN 111983926B
Authority
CN
China
Prior art keywords
ellipsoid
matrix
representing
error
system state
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010893973.8A
Other languages
Chinese (zh)
Other versions
CN111983926A (en
Inventor
丁国强
赵朋朋
凌丹
赵素娜
刘娜
娄泰山
张焕龙
王晓雷
王妍
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhengzhou Ousma Intelligent Control Technology Co ltd
Original Assignee
Zhengzhou University of Light Industry
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhengzhou University of Light Industry filed Critical Zhengzhou University of Light Industry
Priority to CN202010893973.8A priority Critical patent/CN111983926B/en
Publication of CN111983926A publication Critical patent/CN111983926A/en
Application granted granted Critical
Publication of CN111983926B publication Critical patent/CN111983926B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Abstract

The invention provides a maximum co-entropy extended ellipsoid collective filtering method, belongs to the system information processing technology in navigation positioning time service in the field of robots, and particularly relates to a nonlinear state space model of a robot motion carrier constructed by using navigation positioning sensing data. The invention discloses an ellipsoid collective member filtering algorithm based on a minimum mean square error criterion, wherein a maximum covex MCC criterion is introduced, a new system noise expression is obtained by carrying out system model expansion operation by combining a nonlinear system prediction noise error and observation noise, and an error cost function is constructed based on the maximum covex criterion between a system state variable prediction vector and an observation vector, so that an observation updating calculation process of the ellipsoid collective member filtering algorithm is designed, and an ellipsoid collective member filtering algorithm calculation frame based on the maximum covex is constructed; the problem of the calculation stability of the traditional centralized filtering algorithm is effectively solved, and therefore the robot carrier combined navigation and positioning service function is completed.

Description

Maximum co-entropy extended ellipsoid collective filtering method
Technical Field
The invention relates to the technical field of robot navigation and positioning, in particular to a maximum covariance extended ellipsoid collective filtering method.
Background
The objective of ensemble filtering is to update a Feasible Parameter Set (FPS) containing true values of system parameters in real time according to the proposed model, noise bound and observed values, and this method replaces the traditional estimation vector description system with an ensemble, and any Parameter in the Feasible Parameter Set can be considered Feasible. The membership filtering algorithm can be divided into an ellipsoid algorithm, a polyhedron algorithm, a box algorithm, a full-symmetry multi-cell algorithm, a super-parallel acceleration method, an interval algorithm and the like according to different feasible sets of description parameters, and the feasible sets of the parameters are approximately described by adopting corresponding geometric shapes respectively.
The ellipsoid collective-member filtering algorithm is firstly proposed by Schweppe, an ellipsoid approximate description parameter feasible set is adopted for the first time, an algorithm for calculating the vector sum of two ellipsoids is given in the process of prediction updating, an outer-wrapped ellipsoid containing all regions of the two ellipsoids can be obtained, in the process of measurement updating, Schweppe gives an outer-wrapped ellipsoid calculation method for calculating the intersection of the two ellipsoids, the two calculation rules determine the most fundamental theoretical basis of the ellipsoid collective-member filtering algorithm, but the optimization calculation rule of the ellipsoid is not given, then Chernusko provides a conditional formula for predicting the vector sum of the update and calculating the minimum volume of the outer-wrapped ellipsoid, and Maksarov and Norton provide an optimization calculation rule based on the volume of the minimized ellipsoid and the half-axis long square sum of the ellipsoid. The box algorithm is proposed by Milanese and Belforte, the core algorithm of the box algorithm is to determine the outer delimitation of the box, the problem can be converted into a linear programming problem in a linear system, the polyhedron algorithm aims at solving the problem of incompatible elements existing in an ellipsoid and box algorithm, a feasible set of parameters can be accurately described, but the calculation load is large, the calculation amount of the hyper-parallel body algorithm is small, and high estimation precision can be achieved; the fully symmetric multi-cell algorithm can accurately and approximately represent the time-varying characteristics of system parameters. In the ellipsoid set membership filtering algorithm, Fogel and Huang propose a minimum ellipsoid volume rule and a minimum ellipsoid trajectory rule.
Disclosure of Invention
Aiming at the technical problem of computational instability of the existing central difference filtering algorithm, the invention designs the maximum co-entropy cost function of bounded noise items of system noise prediction errors and observation noise errors aiming at the theoretical thought of the information entropy in the machine learning theory on the basis of the assumption that the noise in the ellipsoid collective filtering algorithm is unknown but bounded, thereby constructing a maximum co-entropy extended ellipsoid collective filtering method, applying the maximum co-entropy extended ellipsoid collective filtering method to the calculation of state parameters of a state space model of an extended robot system in a pose model of an autonomous mobile robot, and achieving the purpose of improving the computational stability of the nonlinear collective filtering algorithm.
The technical scheme of the invention is realized as follows:
a maximum covariance extended ellipsoid collective filtering method comprises the following steps:
the method comprises the following steps that firstly, a nonlinear discrete system state space model of the ground-based autonomous mobile robot is constructed, system state variables of a nonlinear discrete system are initialized, and an ellipsoid set of the system state variables is given; the nonlinear discrete system state space model comprises a dynamic motion model equation and a discretization observation model equation;
step two, obtaining the estimated value of the system state variable at the k-1 moment according to the initialization result of the nonlinear discrete system state space model
Figure BDA0002657854200000021
And a system state variable ellipsoidal shape matrix Pk-1And for the system state variable ellipsoid shape matrix Pk-1Performing Cholesky decomposition operation;
step three, adopting Stirling interpolation polynomial to estimate the system state variable at the k-1 th moment
Figure BDA0002657854200000022
Carrying out linear approximation calculation, and taking the linear second-order polynomial as a Lagrange remainder formula of the nonlinear discrete system;
step four, utilizing an ellipsoid to outsource Lagrange remainder sub-type of the nonlinear discrete system to obtain a linearization error, and calculating an outsourcing ellipsoid of the linearization error;
adding the linearization error and the process noise of the nonlinear discrete system to obtain a virtual process noise error, calculating a virtual process noise error ellipsoid, and performing Cholesky decomposition operation on the virtual process noise error ellipsoid;
step six, calculating the state parameter ellipsoid boundary of the system state variable and the predicted value of the system state variable by utilizing a linear ellipsoid collective filtering algorithm
Figure BDA0002657854200000023
An ellipsoid-shaped matrix of the system state variable is predicted according to the virtual process noise error ellipsoid;
seventhly, estimating the system state variable value based on the discretization observation model equation and the k-1 moment
Figure BDA0002657854200000024
Predicting and updating the state variables of the nonlinear dispersion system to obtain a predicted value of the observation vector and a predicted ellipsoidal shape matrix, and calculating a predicted covariance matrix according to the square root of the predicted ellipsoidal shape matrix and the square root of the ellipsoidal shape matrix in the step six;
step eight, predicting values according to the system state variables in the step six
Figure BDA0002657854200000025
Calculating the prediction error of the state variable, and predicting the system state variable in the sixth step
Figure BDA0002657854200000026
Combining the system error expansion equation with an observation equation to obtain a system error expansion equation; obtaining a conversion expansion state model according to a variance matrix constructed by utilizing an expansion noise item of a system error expansion equation;
step nine, constructing a cost function by using an MCC (Motor control center) criterion according to the conversion expansion state model in the step eight, and obtaining the optimal estimation value of the system state variable at the kth moment by calculating the optimal solution of the cost function
Figure BDA0002657854200000027
And step ten, updating the state parameter ellipsoid boundary of the system state variable by using a linear ellipsoid collective filtering algorithm, and calculating a predicted value and a predicted variance matrix of the system state vector at the kth moment according to the state parameter ellipsoid boundary.
The nonlinear discrete system state space model is as follows:
Figure BDA0002657854200000028
wherein x isk∈RnState variable, x, representing time kk-1Representing the state variable at time k-1, f (-) and h (-) each representing a non-linear second order derivative function, qk-1Representing the process noise term at time k-1, rk∈RmRepresenting the observed noise term, y, over timekRepresenting an observation vector;
the ellipsoid set of the system state variables is:
E(a，P)＝{x∈Rn|(x-a)TP-1(x-a)≤1}，
wherein a represents the center of the ellipsoid set, P is a positive qualitative ellipsoid envelope matrix, and the system initial state estimation ellipsoid set is
Figure BDA0002657854200000031
The system state variable estimation value at the k-1 time is subjected to a Stirling interpolation polynomial
Figure BDA0002657854200000032
The expression for the calculation of the linearized approximation is:
Figure BDA0002657854200000033
wherein x iskState variable representing time k, DΔxThe term is called a difference operator;
Figure BDA0002657854200000034
Figure BDA0002657854200000035
wherein, Δ xpRepresenting the estimated deviation, μ, of the decoupled state variable of the systempAs deviation operator, δpIs an averageThe operator(s) is (are) selected,
Figure BDA0002657854200000036
representing the estimation deviation of the system state variable at the k-1 moment, wherein s is an interpolation step length;
said deviation operator mupComprises the following steps:
Figure BDA0002657854200000037
the average operator deltapComprises the following steps:
Figure BDA0002657854200000038
wherein the content of the first and second substances,
Figure BDA0002657854200000039
is a unit vector along the axial direction;
the Lagrange remainder formula is:
Figure BDA00026578542000000310
wherein the content of the first and second substances,
Figure BDA00026578542000000311
indicating Lagrange remainder terms centered on the state estimate at time k-1.
The method for utilizing the ellipsoid to outsource the Lagrange remainder formula of the nonlinear discrete system to obtain the linearization error comprises the following steps:
Figure BDA00026578542000000312
wherein the content of the first and second substances,
Figure BDA0002657854200000041
represents the diagonal elements of the linearized error shape matrix determined by Lagrange's remainder terms at time k-1,
Figure BDA0002657854200000042
representing the non-diagonal elements of the linearization error shape matrix determined by Lagrange remainder items at the k-1 moment, wherein i and j represent the ith row and the jth column of the linearization error shape matrix, the value is i more than or equal to 1, j more than or equal to n, and n is the dimension of the system state vector;
the outer ellipsoid of the linearization error is:
Figure BDA0002657854200000043
wherein the content of the first and second substances,
Figure BDA0002657854200000044
representing a linearized error shape matrix determined by Lagrange's remainder terms.
The virtual process noise error is:
Figure BDA0002657854200000045
wherein the content of the first and second substances,
Figure BDA0002657854200000046
representing the virtual process noise at time k-1,
Figure BDA0002657854200000047
representing a virtual process noise variance matrix, Qk-1A process noise matrix is represented that represents the process noise matrix,
Figure BDA00026578542000000421
a set of add operations is represented and,
Figure BDA00026578542000000420
representing a process noise error optimization factor;
the virtual process noise error ellipsoid is:
Figure BDA0002657854200000048
wherein the content of the first and second substances,
Figure BDA0002657854200000049
representing a virtual process noise at time k;
performing Cholesky decomposition operation on the virtual process noise error ellipsoid to obtain:
Figure BDA00026578542000000410
wherein the content of the first and second substances,
Figure BDA00026578542000000411
representing the virtual process noise variance matrix square root.
The system state variable prediction value
Figure BDA00026578542000000412
Comprises the following steps:
Figure BDA00026578542000000413
wherein
Figure BDA00026578542000000414
Is a unit vector along the axial direction;
the ellipsoid-shaped matrix of the system state variables is:
Figure BDA00026578542000000415
wherein the content of the first and second substances,
Figure BDA00026578542000000416
a matrix of predicted shapes representing the state variables of the system,
Figure BDA00026578542000000417
is represented by P'k，k-1The square root of (a);
Figure BDA00026578542000000418
Figure BDA00026578542000000419
prediction value of the observation vector
Figure BDA0002657854200000051
Wherein the content of the first and second substances,
Figure BDA0002657854200000052
representing a system observation vector predicted value;
prediction covariance matrix Pxy，k，k-1Comprises the following steps:
Figure BDA0002657854200000053
wherein the content of the first and second substances,
Figure BDA0002657854200000054
expressing a system observation equation prediction error matrix;
Figure BDA0002657854200000055
Figure RE-GDA0002694116500000056
the state variable prediction error is:
Figure BDA0002657854200000057
wherein the content of the first and second substances,
Figure BDA0002657854200000058
representing a state variable prediction error;
the system error expansion equation is:
Figure BDA0002657854200000059
wherein the content of the first and second substances,
Figure BDA00026578542000000510
first order difference operator matrix representing a non-linear observation equation, BkA matrix of first order difference operators, R, representing the k-th momentkRepresenting the observation noise term, I representing the identity matrix,
Figure BDA00026578542000000511
is the spreading noise term;
the variance matrix constructed by using the extended noise term of the system error extension equation is as follows:
Figure BDA00026578542000000512
wherein, thetakRepresenting the spread noise variance matrix, SkRepresenting the spread noise variance matrix ΘkCholesky decomposition matrix of (S)r，kRepresents the square root of the observed noise error matrix,
Figure BDA00026578542000000513
representing a spread noise square root matrix;
the transition extended state model is:
Dk＝Wkxk+ek，
wherein the content of the first and second substances,
Figure BDA00026578542000000514
indicating the expanded state at the time of the k-th instant,
Figure BDA00026578542000000515
represents the extended state transition matrix at time k,
Figure BDA0002657854200000061
and is
Figure BDA0002657854200000062
The cost function is:
Figure BDA0002657854200000063
wherein d isi,kIs a matrix DkThe ith row vector ofi,kIs a matrix WkL ═ n + m denotes the matrix DkDimension of, JL(xk) Representing the MCC cost function, GσRepresenting an error cost function;
the optimal estimated value of the system state variable at the k-th moment
Figure BDA0002657854200000064
Comprises the following steps:
Figure BDA0002657854200000065
the cost function is subjected to partial derivation and sorting to obtain:
Figure BDA0002657854200000066
wherein, Cx,k＝diag(Gσ(d1,k-w1,kxk),…,Gσ(dn,k-wn,kxk))，
Cy,k＝diag(Gσ(dn+1,k-wn+1,kxk),…,Gσ(dn+m,k-wn+m,kxk))。
The state parameter ellipsoid boundary for updating the system state variable by using the linear ellipsoid collective filtering algorithm is as follows:
Figure BDA0002657854200000067
wherein, PkRepresenting the system state variable estimation error matrix at time k, EkAn ellipsoid representing the updated system state variables;
the predicted value of the system state vector at the kth moment calculated according to the state parameter ellipsoid boundary is as follows:
Figure BDA0002657854200000068
wherein the content of the first and second substances,
Figure BDA0002657854200000069
representing an ellipsoid collector filter gain matrix;
the prediction variance matrix is:
Figure BDA00026578542000000610
wherein the content of the first and second substances,
Figure BDA00026578542000000611
Figure BDA00026578542000000612
ρk∈(0,1)，
Figure BDA00026578542000000613
is a first order difference operator matrix of the observation equation.
The beneficial effect that this technical scheme can produce: the invention introduces a novel maximum co-entropy MCC criterion in the observation updating step on the basis of the traditional ellipsoid collective filtering algorithm based on the minimum mean square error criterion, obtains a novel system noise expression by carrying out system model expansion operation by combining the nonlinear system prediction noise error and the observation noise, and constructs an error cost function expressed by a second-order information potential energy formula according to the maximum co-entropy criterion based on the system state variable prediction vector and the observation vector, thereby designing the observation updating calculation process of the ellipsoid collective filtering algorithm and constructing a novel ellipsoid collective filtering algorithm calculation frame based on the maximum co-entropy; by utilizing the method disclosed by the invention to carry out the pose calculation simulation verification of the land-based robot, the calculation precision of the method disclosed by the invention is improved, and the calculation stability is obviously improved and enhanced compared with that of the traditional ellipsoid collective filtering algorithm.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
FIG. 1 is a computational flow diagram of the present invention.
FIG. 2 is a schematic diagram of a mobile robot motion model in the method of the present invention.
FIG. 3 is a diagram of the error data calculated by the mobile robot for MCC-SMF according to the present invention.
FIG. 4 is a diagram of the mobile robot carrier trajectory calculation data of the MCC-SMF of the present invention.
Fig. 5 is a graph of the calculated error data of the mobile robot carrier obtained by the EKF algorithm.
Fig. 6 is a graph of the mobile robot carrier trajectory calculation data obtained by the EKF algorithm.
Fig. 7 is a diagram of the mobile robot carrier calculation error data obtained by the SUKF algorithm.
Fig. 8 is a diagram of the mobile robot carrier trajectory calculation data obtained by the SUKF algorithm.
Detailed Description
The technical solutions in the embodiments of the present invention will be described clearly and completely with reference to the accompanying drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without any inventive step, are within the scope of the present invention.
Preliminary knowledge
The co-entropy criterion is a local similarity measurement concept which defines a similarity measurement between two random vectors in a kernel space, and has the advantage of insensitivity to large errors, so that the computation efficiency of the system state variable is not influenced by impulse noise or abnormal data processing. For two random variables X ∈ Rn，Y∈RnWith a joint probability density function of pX，Y(x, y), then the coventropy of two random variables is defined as,
Figure BDA0002657854200000081
kappa (X, Y) in the definition of coventropy is a radial kernel function, E [ kappa (X, Y) ] represents the desired operator, the radial kernel function is generally represented by a Gaussian symmetric kernel function,
Figure BDA0002657854200000082
wherein x-y-e represents an error vector of two variable vectors, and σ represents a bandwidth value σ > 0 of the kernel function; for an asymmetric gaussian kernel distribution, however, its kernel function can be expressed as,
Figure BDA0002657854200000083
σ + and σ -as defined herein represent the "+" and "-" portions of the error vector, respectively, then asymmetric coventropy may be defined as,
Figure BDA0002657854200000088
for limited data sets
Figure BDA0002657854200000084
A simple sample mean estimator can be used to approximate the coventropy expression of a continuous gaussian kernel as,
Figure BDA0002657854200000085
further obtains the Taylor series expansion expression as,
Figure BDA0002657854200000086
then, if the state parameter x and the output variable y of the state space model are considered, the optimal estimated target value of the system state variable obtained by performing iterative recursive optimal filtering calculation based on the maximum covariance criterion can be expressed as,
Figure BDA0002657854200000087
then, a cost function can be constructed by utilizing a maximum co-entropy criterion to carry out the design work of the nonlinear system state space model state parameter filtering algorithm.
Aiming at the computational instability of a central differential filtering algorithm, the filtering computation criterion is revised from Minimum Mean Square Error (MMSE) to maximum negotiation entropy criterion MCC, so that a novel maximum negotiation entropy ellipsoid collective filtering algorithm is designed and obtained, and is applied to an autonomous mobile robot pose model to develop the state parameter computation of a robot system state space model.
The invention discloses a method for constructing a computing frame of an ellipsoid collective filtering algorithm based on maximum entropy, which is based on a traditional ellipsoid collective filtering algorithm based on a minimum mean square error criterion, faces to a nonlinear system state space model, reserves the computing process of a time updating prediction step of the traditional ellipsoid collective filtering algorithm, introduces a novel maximum entropy negotiation MCC criterion in an observation updating step, obtains a novel system noise expression by carrying out system model expansion operation by combining nonlinear system prediction noise error and observation noise, constructs an error cost function expressed by a second-order information potential energy formula according to the maximum entropy criterion based on a system state variable prediction vector and an observation vector, and designs the observation updating computing process of the ellipsoid collective filtering algorithm based on the maximum entropy. The algorithm is used for carrying out the pose calculation simulation verification of the ground-based robot, the calculation precision of the method is improved, and the calculation stability is obviously improved compared with that of the traditional ellipsoid collective filtering algorithm.
The maximum entropy criterion is a method for calculating the maximum value of error information based on a Renyis entropy formula, an information potential energy concept in a machine learning theory is introduced, a second-order information potential energy expression is abstracted to describe the prediction error of a nonlinear system state variable in an ellipsoid collective filtering algorithm, a nonlinear system extended error state model is obtained together with an observation equation of a nonlinear system state space model, the noise variance of the extended error state model is calculated, Cholesky decomposition is carried out on the noise variance model to obtain a square root variance positive definite matrix of extended noise, the extended error state model is transformed and sorted by using the square root noise variance matrix to obtain a prediction error and observation noise combined error expression, then a prediction error cost function of the ellipsoid collective filtering algorithm is synthesized by using a second-order information expression of the maximum entropy criterion, then the optimal calculation of the system state variable is obtained by using the minimum maximum entropy negotiation cost function, calculating partial differential of a minimized error entropy cost function and setting the partial differential to be 0 to obtain the minimum error entropy cost function, simultaneously realizing an explicit expression of system state variable optimization calculation by utilizing a matrix inverse theorem, setting a small parameter in computer algorithm compilation to judge a system state variable estimation value of each iterative calculation, and if the judgment expression is true, continuing to calculate a system state variable estimation variance matrix; otherwise, the calculation process of the previous step is continued, and finally the calculation of the system state variable estimation variance matrix is obtained.
As shown in fig. 1, an embodiment of the present invention provides a maximum covariance extended ellipsoid membership filtering method, which includes the following steps:
the method comprises the following steps that firstly, a nonlinear discrete system state space model of the ground-based autonomous mobile robot is constructed, system state variables of a nonlinear discrete system are initialized, and an ellipsoid set of the system state variables is given; the nonlinear discrete system state space model comprises a dynamic motion model equation and a discretization observation model equation;
the nonlinear discrete system state space model is as follows:
Figure BDA0002657854200000091
wherein x isk∈RnState variable, x, representing time kk-1Representing the state variable at time k-1, f (-) and h (-) each representing a non-linear second order derivative function, qk-1Representing the process noise term at time k-1, rk∈RmRepresenting the observed noise term, y, over timekRepresenting an observation vector; q. q.sk-1∈RnAnd rk∈RmRepresenting process noise and observation noise over time, and satisfying Unknown But Bounded (UBB) assumptions,
Figure BDA0002657854200000101
note qk∈(0，Qk) And rk∈(0，Rk). Initial state x of the system0Belonging to a known bounded set x0∈X0The set may be determined from a priori knowledge of the system state, while for the measurement sequence vector
Figure BDA0002657854200000102
Then the state feasible set of the ellipsoid membership filtering algorithm at time k is defined as XkIt consists of all possible state points that are consistent with all available information, including the system model, noise assumptions, and initial state set.
The ellipsoid set of the system state variables is:
E(a，P)＝{x∈Rn|(x-a)TP-1(x-a)≤1} (9)，
wherein a represents the center of the ellipsoid set, P is a positive qualitative ellipsoid envelope matrix, and the system initial state estimation ellipsoid set is
Figure BDA0002657854200000103
The system state ellipsoid set obtained by k-1 time estimation is
Figure BDA0002657854200000104
The iterative process of the non-linear ellipsoid membership filtering algorithm at the time k consists of steps two to eight.
Step two, obtaining the estimated value of the system state variable at the k-1 moment according to the initialization result of the nonlinear discrete system state space model
Figure BDA0002657854200000105
And a system state variable ellipsoidal shape matrix Pk-1And for the system state variable ellipsoid shape matrix Pk-1A Cholesky decomposition operation is performed,
Figure BDA0002657854200000106
the uncertainty interval of the state component at time k-1 is set to,
Figure BDA0002657854200000107
wherein, i is 1, 2., n,
Figure BDA0002657854200000108
denotes Sk-1The (i, i) th diagonal element.
Thirdly, in the process of designing the nonlinear system state space model, carrying out linear approximation calculation on the nonlinear system function of the moving target, wherein Stirling interpolation polynomial is adopted to carry out linear approximation calculation on the nonlinear function, and the Stirling interpolation polynomial is adopted to carry out linear approximation calculation on the estimated value of the system state variable at the k-1 moment
Figure BDA0002657854200000109
Performing linear approximation calculation, and taking the linearized second-order polynomial as a Lagrange remainder formula of the nonlinear discrete system;
the system state variable estimation value at the k-1 time is subjected to a Stirling interpolation polynomial
Figure BDA00026578542000001010
The expression for the calculation of the linearized approximation is:
Figure BDA00026578542000001011
wherein x iskState variable representing time k, DΔxThe term is called a difference operator;
Figure BDA00026578542000001012
Figure BDA00026578542000001013
wherein, Δ xpRepresenting the estimated deviation, μ, of the decoupled state variable of the systempAs deviation operator, δpIn order to average the operators, the operator is,
Figure BDA0002657854200000111
representing the estimation deviation of the system state variable at the k-1 moment, wherein s is an interpolation step length;
said deviation operator mupComprises the following steps:
Figure BDA0002657854200000112
the average operator deltapComprises the following steps:
Figure BDA0002657854200000113
wherein the content of the first and second substances,
Figure BDA0002657854200000114
is a unit vector along the axial direction;
as can be seen from the approximation expression of the Stirling interpolation polynomial, the computation accuracy of the Stirling interpolation expansion is higher than that of the Taylor series expansion, and the accuracy can be controlled by the interpolation step length s. Taking the first two terms of the Stirling interpolation polynomial as linearized approximations of the nonlinear system process function, wherein the Lagrange remainder formula is as follows:
Figure BDA0002657854200000115
wherein the content of the first and second substances,
Figure BDA0002657854200000116
indicating Lagrange remainder terms centered on the state estimate at time k-1.
Step four, utilizing an ellipsoid to outsource Lagrange remainder sub-type of the nonlinear discrete system to obtain a linearization error, and calculating an outsourcing ellipsoid of the linearization error;
the method for utilizing the ellipsoid to outsource the Lagrange remainder formula of the nonlinear discrete system to obtain the linearization error comprises the following steps:
Figure BDA0002657854200000117
wherein the content of the first and second substances,
Figure BDA0002657854200000118
represents the diagonal elements of the linearized error shape matrix determined by Lagrange's remainder terms at time k-1,
Figure BDA0002657854200000119
representing the non-diagonal elements of the linearization error shape matrix determined by Lagrange remainder items at the k-1 time, i and j representing the ith row and the jth column of the linearization error shape matrix, and the value of the i and the jth columns is more than or equal to 1i, j is less than or equal to n, and n is the dimension of the system state vector;
the outer ellipsoid of the linearization error is:
Figure BDA00026578542000001110
wherein
Figure BDA00026578542000001111
Representing a linearized error shape matrix determined by Lagrange's remainder terms. .
Adding the linearization error and the process noise of the nonlinear discrete system to obtain a virtual process noise error, calculating a virtual process noise error ellipsoid, and performing Cholesky decomposition operation on the virtual process noise error ellipsoid;
the virtual process noise error is:
Figure BDA0002657854200000121
wherein the content of the first and second substances,
Figure BDA0002657854200000122
representing the virtual process noise at time k-1,
Figure BDA0002657854200000123
representing a virtual process noise variance matrix, Qk-1A process noise matrix is represented that represents the process noise matrix,
Figure BDA0002657854200000124
a set of add operations is represented and,
Figure BDA0002657854200000125
representing a process noise error optimization factor;
the virtual process noise error ellipsoid is:
Figure BDA0002657854200000126
wherein the content of the first and second substances,
Figure BDA0002657854200000127
representing a virtual process noise at time k;
performing Cholesky decomposition operation on the virtual process noise error ellipsoid to obtain:
Figure BDA0002657854200000128
wherein the content of the first and second substances,
Figure BDA0002657854200000129
representing the virtual process noise variance matrix square root.
Step six, calculating the state parameter ellipsoid boundary of the system state variable and the predicted value of the system state variable by utilizing a linear ellipsoid collective filtering algorithm
Figure BDA00026578542000001210
An ellipsoid-shaped matrix of the system state variable is predicted according to the virtual process noise error ellipsoid; calculating the boundary of the prediction state parameter ellipsoid by using the prediction step of the linear ellipsoid collective filtering algorithm, which is a linearized prediction ellipsoid
Figure BDA00026578542000001211
And a virtual process noise direct sum calculation process;
the system state variable prediction value
Figure BDA00026578542000001212
Comprises the following steps:
Figure BDA00026578542000001213
wherein
Figure BDA00026578542000001214
Is a unit vector along the axial direction;
the ellipsoid-shaped matrix of the system state variables is:
Figure BDA00026578542000001215
wherein the content of the first and second substances,
Figure BDA00026578542000001216
a matrix of predicted shapes representing the state variables of the system,
Figure BDA00026578542000001217
is represented by P'k，k-1The square root of (a);
Figure BDA00026578542000001218
Figure BDA00026578542000001219
seventhly, estimating the system state variable value based on the discretization observation model equation and the k-1 moment
Figure BDA00026578542000001220
Predicting and updating the state variables of the nonlinear dispersion system to obtain a predicted value of the observation vector and a predicted ellipsoidal shape matrix, and calculating a predicted covariance matrix according to the square root of the predicted ellipsoidal shape matrix and the square root of the ellipsoidal shape matrix in the step six;
on the basis of predicting and updating the state variables of the nonlinear system, one-step prediction is carried out on the observation vector, and the prediction value of the obtained observation vector is as follows:
Figure BDA0002657854200000131
wherein the content of the first and second substances,
Figure BDA0002657854200000132
representing a system observation vector predicted value;
prediction covariance matrix Pxy，k，k-1Comprises the following steps:
Figure BDA0002657854200000133
wherein the content of the first and second substances,
Figure BDA0002657854200000134
expressing a system observation equation prediction error matrix;
Figure BDA0002657854200000135
Figure BDA0002657854200000136
step eight, predicting values according to the system state variables in the step six
Figure BDA0002657854200000137
Calculating the prediction error of the state variable, and predicting the system state variable in the sixth step
Figure BDA0002657854200000138
Combining the system error expansion equation with an observation equation to obtain a system error expansion equation; obtaining a conversion expansion state model according to a variance matrix constructed by utilizing an expansion noise item of a system error expansion equation;
the state variable prediction error is:
Figure BDA0002657854200000139
wherein the content of the first and second substances,
Figure BDA00026578542000001310
representing a state variable prediction error;
the system error expansion equation is:
Figure BDA00026578542000001311
wherein the content of the first and second substances,
Figure BDA00026578542000001312
first order difference operator matrix representing a non-linear observation equation, BkA matrix of first order difference operators, R, representing the k-th momentkRepresenting the observation noise term, I representing the identity matrix,
Figure BDA00026578542000001313
is the spreading noise term;
the variance matrix constructed by using the extended noise term of the system error extension equation is as follows:
Figure BDA00026578542000001314
wherein, thetakRepresenting the spread noise variance matrix, SkRepresenting the spread noise variance matrix ΘkCholesky decomposition matrix of (S)r，kRepresents the square root of the observed noise error matrix,
Figure BDA0002657854200000141
representing a spread noise square root matrix; (ii) a
The transition extended state model is:
Dk＝Wkxk+ek (30)，
wherein the content of the first and second substances,
Figure BDA0002657854200000142
indicating the expanded state at the time of the k-th instant,
Figure BDA0002657854200000143
represents the extended state transition matrix at time k,
Figure BDA0002657854200000144
and is
Figure BDA0002657854200000145
Step nine, constructing a cost function by using an MCC (Motor control center) criterion according to the conversion expansion state model in the step eight, and obtaining the optimal estimation value of the system state variable at the kth moment by calculating the optimal solution of the cost function
Figure BDA00026578542000001414
The cost function is:
Figure BDA0002657854200000146
wherein d isi，kIs a matrix DkThe ith row vector ofi，kIs a matrix WkL ═ n + m denotes the matrix DkDimension of, JL(xk) Representing the MCC cost function, GσRepresenting an error cost function; (ii) a
Optimal estimation of system state variables
Figure BDA0002657854200000147
Is the optimal solution of equation (31), and therefore, the optimal estimated value of the system state variable at the k-th time
Figure BDA0002657854200000148
Comprises the following steps:
Figure BDA0002657854200000149
by solving partial differential for equation (31)
Figure BDA00026578542000001410
The expression is obtained as follows,
Figure BDA00026578542000001411
further finishing to obtain:
Figure BDA00026578542000001412
wherein, Cx，k＝diag(Gσ(d1，k-w1，kxk)，…，Gσ(dn，k-wn，kxk))，
Cy，k＝diag(Gσ(dn+1，k-wn+1，kxk)，…，Gσ(dn+m，k-wn+m，kxk))。
Step ten, updating the state parameter ellipsoid boundary of the system state variable by utilizing a linear ellipsoid collective filtering algorithm
Figure BDA00026578542000001413
Wherein, PkRepresenting the system state variable estimation error matrix at time k, EkAn ellipsoid representing an updated system state variable; the essence is to predict the state ellipsoid
Figure BDA0002657854200000151
And observation set
Figure BDA0002657854200000152
The calculation of the straight intersection and the intersection is carried out,
Figure BDA0002657854200000153
Figure BDA0002657854200000154
calculating the predicted value and the prediction variance matrix of the system state vector at the k moment according to the ellipsoid boundary of the state parameters,
Figure BDA0002657854200000155
the prediction variance matrix is:
Figure BDA0002657854200000156
wherein the content of the first and second substances,
Figure BDA0002657854200000157
Figure BDA0002657854200000158
is a first order difference operator matrix of the observation equation.
The method has the advantages that the method adopts the Stirling interpolation polynomial to implement the linearization operation, effectively avoids the complex calculation of the first-order Jacobian matrix and the second-order Hessian matrix of the Taylor series expansion, and reduces the calculation complexity of the algorithm; the calculation precision can be controlled by utilizing the interpolation step length s; compared with the traditional nonlinear ensemble filtering algorithm based on Taylor series expansion, the method provided by the invention has higher calculation precision.
In addition, the algorithm introduces four parameters, interpolation step length s and three scale factor parameters
Figure BDA00026578542000001516
βk-1And ρkThe numerical determination method is as follows:
for the interpolation step length s parameter, in general, if the system state vector satisfies the Gauss distribution,
Figure BDA0002657854200000159
to satisfy this condition, the estimated variance matrix of the system state vector computed at each iteration implements a cholesky decomposition, P ═ SSTTherefore, decoupling transformation operation is carried out on the system state vector to enable the system state vector to meet Gauss distribution conditions.
Scale factor parameter
Figure BDA00026578542000001510
And betak-1Involving the outsourcing of two ellipsoidal direct sumsThe method has simple solving form, and has stronger performance robustness compared with the optimization criterion of minimizing the volume of the outer-wrapped ellipsoid. Namely have
Figure BDA00026578542000001511
Thereby can adopt the formula
Figure BDA00026578542000001512
Obtaining optimal scale factor parameters
Figure BDA00026578542000001513
And betak-1。
Scale factor parameter
Figure BDA00026578542000001514
Requires E (0, Q)k-1) And
Figure BDA00026578542000001515
and (3) calculating the direct sum, and then calculating the formula of the calculation criterion as follows:
Figure BDA0002657854200000161
the optimal calculation formula is
Figure BDA0002657854200000162
For the scale factor parameter betak-1Two ellipsoids are required
Figure BDA0002657854200000163
And
Figure BDA0002657854200000164
the calculation formula of the variance matrix under the condition of considering the updating of the observation vector is as follows:
Figure BDA0002657854200000165
thereby obtaining the scale factor parameter betak-1Is calculated by the formula
Figure RE-GDA0002694116500000166
In the iterative calculation process, an observation set SyThe form is generally complex, resulting in a system state vector variance matrix PkWhether using the minimized ellipsoid volume method or the minimized ellipsoid trajectory criterion, the calculation complexity of (2) makes the scale factor parameter ρkThe optimization calculation is difficult, even an analytic solution cannot be obtained, and the calculation complexity is high if a numerical calculation method is adopted. In the present invention, the minimum performance index delta is adoptedkIs calculated in upper bound form
Figure BDA0002657854200000167
Thus, the scale factor parameter ρ can be obtainedkA suboptimum calculation formula
Figure BDA0002657854200000168
Wherein p ismIs a matrix
Figure BDA0002657854200000169
Maximum singular value of cmIs that
Figure BDA00026578542000001610
The maximum singular value of the matrix.
Examples of the applications
In order to verify the calculation efficiency of the minimum error entropy center differential filtering algorithm provided by the invention, the method is utilized to carry out simulation verification calculation on a land-based robot positioning system model to prove the effectiveness and the calculation advantages of the method, and simulation verification test data are provided. Here consider a groundA surface mobile robot system adopts a front wheel driving mode, as shown in figure 2, the steering angle of a front wheel is defined as alpha, the anticlockwise direction is defined as the positive direction, and a robot coordinate system xy is adoptedRobotRelative to the ground coordinate system xygroundWith steering angle psi, counter-clockwise positive and rear wheel speed defined as vrearwheel. The distance R between an observation point and the center of the rear wheel and the wheel base L of the front wheel and the rear wheel satisfy
Figure BDA00026578542000001611
So that the observation point distance can be obtained,
Figure BDA00026578542000001612
simultaneously, the expression of the rear wheel speed can be obtained as follows:
Figure BDA0002657854200000171
the steering angle equation of the robot can be obtained by arranging,
Figure BDA0002657854200000172
is finished to obtain
Figure BDA0002657854200000173
If the movement speed of the mobile robot system is considered, the dynamic equation of the mobile robot system can be obtained in the robot coordinate system as
Figure BDA0002657854200000174
The dynamic equation of the robot system is converted into a ground coordinate system, and the final mobile equation of the mobile robot system in the ground coordinate system can be obtained
Figure BDA0002657854200000175
The parameter alpha (t) in the system equation can be used as a modulation parameter and used as an input variable of the system to generate a control parameter of the front wheel, which meets the control law
Figure BDA0002657854200000176
The parameter psi heredesRepresenting the desired heading angle and the parameter ψ represents the current moving heading angle, the desired heading angle can be expressed as
Figure BDA0002657854200000177
Generally, the steering angle ranges from (+/-pi/4), and the G parameter influences the speed of the steering of the robot. For the purpose of carrying out system state variable estimation and motion path tracking on a motion equation of a mobile robot system, a plurality of sensors are used for sensing the coordinate position of the system, such as a GPS or a forward direction coding direction finder and other equipment for completing target tracking observation, and the two-direction position coordinates of the mobile robot in a ground coordinate system are selected as observation variables, so that the observation equation is linear and can be directly obtained. Consider a system initial variance matrix of
Figure BDA0002657854200000181
In addition, system parameters are set as that the wheel base L of the front wheel and the rear wheel of the mobile robot is 2m, the control law gain G is 2, the moving speed is kept as v is 1m/s, and the sampling time interval delta t is 0.1 s; assuming that only the heading angle psi has interference noise in the process variable, the process noise variance can be set to Q0.052. The observed variable is the mobile robot position coordinates, so the observed noise variance matrix can be set to
Figure BDA0002657854200000182
Therefore, the moving track simulation result of the mobile robot system can be obtained, and the results of the method, the EKF algorithm and the SUKF algorithm are shown in FIGS. 3-8. Comparing the MCC-SMF algorithm with the SUKF algorithm, the invention has the advantages of better calculation stability of the MCC-SMF algorithm, high calculation convergence speed and obviously improved and improved calculation precision.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, but rather as the subject matter of the invention is to be construed in all aspects and equivalents thereof.

Claims (8)

1. A maximum covariance extended ellipsoid collective filtering method is characterized by comprising the following steps:
the method comprises the following steps that firstly, a nonlinear discrete system state space model of the ground-based autonomous mobile robot is constructed, system state variables of a nonlinear discrete system are initialized, and an ellipsoid set of the system state variables is given; the nonlinear discrete system state space model comprises a dynamic motion model equation and a discretization observation model equation;
step two, obtaining the estimated value of the system state variable at the k-1 moment according to the initialization result of the nonlinear discrete system state space model
Figure FDA0003533989710000011
And a system state variable ellipsoidal shape matrix Pk-1And for the system state variable ellipsoid shape matrix Pk-1Performing Cholesky decomposition operation;
step three, adopting Stirling interpolation polynomial to estimate the system state variable at the k-1 th moment
Figure FDA0003533989710000012
Performing linear approximation calculation, and performing linear approximationThe second-order polynomial is used as Lagrange remainder of the nonlinear discrete system;
step four, utilizing an ellipsoid to outsource Lagrange remainder sub-type of the nonlinear discrete system to obtain a linearization error, and calculating an outsourcing ellipsoid of the linearization error;
adding the linearization error and the process noise of the nonlinear discrete system to obtain a virtual process noise error, calculating a virtual process noise error ellipsoid, and performing Cholesky decomposition operation on the virtual process noise error ellipsoid;
step six, calculating the state parameter ellipsoid boundary of the system state variable and the predicted value of the system state variable by utilizing a linear ellipsoid collective filtering algorithm
Figure FDA0003533989710000013
An ellipsoid-shaped matrix of the system state variable is predicted according to the virtual process noise error ellipsoid;
seventhly, estimating the system state variable value based on the discretization observation model equation and the k-1 moment
Figure FDA0003533989710000014
Predicting and updating the state variables of the nonlinear discrete system to obtain a predicted value of the observation vector and a predicted ellipsoidal matrix, and calculating a predicted covariance matrix according to the square root of the predicted ellipsoidal matrix and the square root of the ellipsoidal matrix in the step six;
step eight, predicting values according to the system state variables in the step six
Figure FDA0003533989710000015
Calculating the prediction error of the state variable, and predicting the system state variable in the sixth step
Figure FDA0003533989710000016
Combining the system error expansion equation with an observation equation to obtain a system error expansion equation; obtaining a conversion expansion state model according to a variance matrix constructed by utilizing an expansion noise item of a system error expansion equation;
step nine, according to the conversion expansion state model in the step eight, constructing a cost function by using the maximum covariance MCC criterion, and obtaining the optimal estimation value of the system state variable at the kth moment by calculating the optimal solution of the cost function
Figure FDA0003533989710000017
The cost function is:
Figure FDA0003533989710000018
wherein d isi,kIs a matrix DkThe ith row vector ofi,kIs a matrix WkL ═ n + m denotes the matrix DkThe dimension(s) of (a) is,
Figure FDA0003533989710000019
representing the maximum co-entropy MCC cost function, GσRepresenting an error cost function;
the optimal predicted value x of the system state variable at the kth momentk,k-1Comprises the following steps:
Figure FDA0003533989710000021
the cost function is subjected to partial derivation and sorting to obtain:
Figure FDA0003533989710000022
wherein the content of the first and second substances,
Figure FDA0003533989710000023
Figure FDA0003533989710000024
step ten, updating the state parameter ellipsoid boundary of the system state variable by using a linear ellipsoid collective filtering algorithm, and calculating a predicted value and a predicted variance matrix of the system state vector at the kth moment according to the state parameter ellipsoid boundary;
the state parameter ellipsoid boundary for updating the system state variable by using the linear ellipsoid collective filtering algorithm is as follows:
Figure FDA0003533989710000025
wherein, PkRepresenting the system state variable estimation error matrix at time k, EkAn ellipsoid representing the updated system state variables;
the estimation value of the system state vector at the kth moment calculated according to the state parameter ellipsoid boundary is as follows:
Figure FDA0003533989710000026
wherein the content of the first and second substances,
Figure FDA0003533989710000027
representing an ellipsoidal collector-driver filter gain matrix, Pk,k-1Ellipsoidal-shaped matrix, y, representing system state variableskRepresenting an observation vector, h (-) representing a non-linear second order derivative function, xk,k-1The optimal predicted value of the system state variable at the kth moment is represented;
the system state variable estimation error matrix at the k-th moment is as follows:
Figure FDA0003533989710000028
wherein the content of the first and second substances,
Figure FDA0003533989710000029
Figure FDA00035339897100000210
Hk＝D△xh(xk,k-1) Is a first order difference operator matrix of the observation equation, D△xReferred to as difference operators.
2. The maximum covariance extended ellipsoid membership filtering method of claim 1, wherein the nonlinear discrete system state space model is:
Figure FDA00035339897100000211
wherein x isk∈RnState variable, x, representing time kk-1Representing the state variable at time k-1, f (-) and h (-) each representing a non-linear second order derivative function, qk-1Representing the process noise term at time k-1, rk∈RmRepresenting the observed noise term, y, over timekRepresenting an observation vector;
the ellipsoid set of the system state variables is:
E(a,P)＝{x∈Rn|(x-a)TP-1(x-a)≤1}，
wherein a represents the center of the ellipsoid set, P is a positive qualitative ellipsoid envelope matrix, and the system initial state estimation ellipsoid set is
Figure FDA0003533989710000031
3. The maximum covariance spreading ellipsoid collective filtering method of claim 2, wherein the system state variable estimation value at the k-1 th time is obtained by using Stirling interpolation polynomial
Figure FDA0003533989710000032
The expression for the calculation of the linearized approximation is:
Figure FDA0003533989710000033
wherein x iskState variable representing time k, D△xReferred to as difference operator;
Figure FDA0003533989710000034
Figure FDA0003533989710000035
wherein, Δ xp'Representing the estimated deviation, μ, of the decoupled state variable of the systemp'As deviation operator, δp'In order to average the operators, the operator is,
Figure FDA0003533989710000036
representing the estimation deviation of the system state variable at the k-1 moment, wherein s is an interpolation step length;
said deviation operator mup'Comprises the following steps:
Figure FDA0003533989710000037
the average operator deltap'Comprises the following steps:
Figure FDA0003533989710000038
wherein the content of the first and second substances,
Figure FDA0003533989710000039
is a unit vector along the axial direction;
the Lagrange remainder formula is:
Figure FDA00035339897100000310
wherein the content of the first and second substances,
Figure FDA0003533989710000041
indicating Lagrange remainder terms centered on the state estimate at time k-1.
4. The maximum covariance spreading ellipsoid membership filtering method according to claim 3, wherein the Lagrange remainder sub-formula of the nonlinear discrete system is outsourced by using an ellipsoid to obtain a linearization error as follows:
Figure FDA0003533989710000042
wherein the content of the first and second substances,
Figure FDA0003533989710000043
represents the diagonal elements of the linearized error shape matrix determined by Lagrange's remainder terms at time k-1,
Figure FDA0003533989710000044
representing the non-diagonal elements of the linearization error shape matrix determined by Lagrange remainder items at the k-1 moment, wherein i and j represent the ith row and the jth column of the linearization error shape matrix, the value is i more than or equal to 1, j more than or equal to n, and n is the dimension of the system state vector;
the outer ellipsoid of the linearization error is:
Figure FDA0003533989710000045
wherein the content of the first and second substances,
Figure FDA0003533989710000046
representing a linearized error shape matrix determined by Lagrange's remainder terms.
5. The maximum covariance spreading ellipsoid membership filtering method of claim 4, wherein the virtual process noise error is:
Figure FDA0003533989710000047
wherein the content of the first and second substances,
Figure FDA0003533989710000048
representing the virtual process noise at time k-1,
Figure FDA0003533989710000049
representing a virtual process noise variance matrix, Qk-1A process noise matrix is represented that represents the process noise matrix,
Figure FDA00035339897100000410
represents a set addition operation, βQk-1E (0,1) represents a process noise error optimization factor;
the virtual process noise error ellipsoid is:
Figure FDA00035339897100000411
wherein the content of the first and second substances,
Figure FDA00035339897100000412
representing virtual process noise at time k;
performing Cholesky decomposition operation on the virtual process noise error ellipsoid to obtain:
Figure FDA00035339897100000413
wherein the content of the first and second substances,
Figure FDA00035339897100000414
representing the virtual process noise variance matrix square root.
6. The maximum covariance spreading ellipsoid membership filtering method of claim 5, wherein the system state variable prediction value
Figure FDA00035339897100000415
Comprises the following steps:
Figure FDA00035339897100000416
wherein
Figure FDA00035339897100000417
Is a unit vector along the axial direction;
the ellipsoid-shaped matrix of the system state variables is:
Figure FDA0003533989710000051
wherein the content of the first and second substances,
Figure FDA0003533989710000052
a matrix of predicted shapes representing the state variables of the system,
Figure FDA0003533989710000053
represents P'k,k-1The square root of (a);
Figure FDA0003533989710000054
Figure FDA0003533989710000055
7. the maximum co-entropy extended ellipsoid membership filtering method of claim 6, wherein a predictor of the observation vector
Figure FDA0003533989710000056
Wherein the content of the first and second substances,
Figure FDA0003533989710000057
representing a system observation vector predicted value;
prediction covariance matrix Pxy,k,k-1Comprises the following steps:
Figure FDA0003533989710000058
wherein the content of the first and second substances,
Figure FDA0003533989710000059
expressing a system observation equation prediction error matrix;
Figure FDA00035339897100000510
Figure FDA00035339897100000511
8. the maximum covariance spreading ellipsoid membership filtering method of claim 7, wherein the state variable prediction error is:
Figure FDA00035339897100000512
wherein the content of the first and second substances,
Figure FDA00035339897100000513
representing a state variable prediction error;
the system error expansion equation is:
Figure FDA00035339897100000514
wherein the content of the first and second substances,
Figure FDA00035339897100000515
first order difference operator matrix representing a non-linear observation equation, BkA matrix of first order difference operators, R, representing the k-th momentkRepresenting the observation noise term, I representing the identity matrix,
Figure FDA0003533989710000061
is the spreading noise term;
the variance matrix constructed by using the extended noise term of the system error extension equation is as follows:
Figure FDA0003533989710000062
wherein, thetakRepresenting the spread noise variance matrix, SkRepresenting the spread noise variance matrix ΘkCholesky decomposition matrix of (S)r,kRepresents the square root of the observed noise error matrix,
Figure FDA0003533989710000063
representing a spread noise square root matrix;
the transition extended state model is:
Dk＝Wkxk+ek，
wherein the content of the first and second substances,
Figure FDA0003533989710000064
indicating the expanded state at the time of the k-th instant,
Figure FDA0003533989710000065
represents the extended state transition matrix at time k,
Figure FDA0003533989710000066
and is
Figure FDA0003533989710000067
CN202010893973.8A 2020-08-31 2020-08-31 Maximum co-entropy extended ellipsoid collective filtering method Active CN111983926B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010893973.8A CN111983926B (en) 2020-08-31 2020-08-31 Maximum co-entropy extended ellipsoid collective filtering method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010893973.8A CN111983926B (en) 2020-08-31 2020-08-31 Maximum co-entropy extended ellipsoid collective filtering method

Publications (2)

Publication Number Publication Date
CN111983926A CN111983926A (en) 2020-11-24
CN111983926B true CN111983926B (en) 2022-04-12

Family

ID=73441363

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010893973.8A Active CN111983926B (en) 2020-08-31 2020-08-31 Maximum co-entropy extended ellipsoid collective filtering method

Country Status (1)

Country Link
CN (1) CN111983926B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112683265B (en) * 2021-01-20 2023-03-24 中国人民解放军火箭军工程大学 MIMU/GPS integrated navigation method based on rapid ISS collective filtering
CN112883508B (en) * 2021-01-22 2024-03-08 江南大学 Parallel spatial filtering-based spring damping system state estimation method
CN113110548B (en) * 2021-04-21 2023-05-12 北京控制工程研究所 Design method for spacecraft ellipsoid set evolution
CN113950018B (en) * 2021-10-13 2023-10-27 华东理工大学 Asynchronous multi-sensor network system and global ellipsoid state estimation method
CN114353805B (en) * 2022-01-13 2022-10-21 中国人民解放军国防科技大学 Robot cluster navigation method, device and equipment based on manual assistance
CN116518983B (en) * 2023-07-05 2023-10-17 西安羚控电子科技有限公司 Self-adaptive fusion method and device for mobile robot positioning

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2016176684A1 (en) * 2015-04-30 2016-11-03 The Regents Of The University Of California Entropy field decomposition for image analysis
CN105222780B (en) * 2015-09-07 2016-08-24 郑州轻工业学院 A kind of ellipsoid set-membership filtering method approached based on Stirling interpolation polynomial
CN105740947B (en) * 2016-02-01 2018-01-12 金陵科技学院 Robot performance's optimization method based on Pareto entropy particle cluster algorithms
CN106767780B (en) * 2016-11-28 2017-10-17 郑州轻工业学院 The extension ellipsoid set-membership filtering method approached based on Chebyshev polynomial interopolations
CN108508463B (en) * 2018-03-28 2020-02-18 郑州轻工业学院 Fourier-Hermite orthogonal polynomial based extended ellipsoid collective filtering method
CN109901153B (en) * 2019-03-29 2022-11-01 西安电子科技大学 Target track optimization method based on information entropy weight and nearest neighbor data association

Also Published As

Publication number Publication date
CN111983926A (en) 2020-11-24

Similar Documents

Publication Publication Date Title
CN111983926B (en) Maximum co-entropy extended ellipsoid collective filtering method
CN109885883B (en) Unmanned vehicle transverse motion control method based on GK clustering algorithm model prediction
CN111983927B (en) Ellipsoid collective filtering method for maximum covariance MCC (MCC) criterion
CN111258218B (en) Intelligent vehicle path tracking method based on maximum correlation entropy criterion
CN111783307B (en) Hypersonic aircraft state estimation method
Xu et al. Enhanced bioinspired backstepping control for a mobile robot with unscented Kalman filter
CN110095983B (en) Mobile robot prediction tracking control method based on path parameterization
CN113296514B (en) Local path optimization method and system based on sparse banded structure
CN111969979B (en) Minimum error entropy CDKF filter method
Gu et al. Trajectory planning and tracking control of a ground mobile robot: A reconstruction approach towards space vehicle
Qin et al. An improved real-time slip model identification method for autonomous tracked vehicles using forward trajectory prediction compensation
Cossette et al. The complex-step derivative approximation on matrix lie groups
Wu et al. LiDAR based trajectory-tracking of an autonomous differential drive mobile robot using fuzzy sliding mode controller
Demim et al. Visual SVSF-SLAM algorithm based on adaptive boundary layer width
Jiang et al. Learning based predictive error estimation and compensator design for autonomous vehicle path tracking
CN107292445A (en) A kind of space non-cooperative target pose Forecasting Methodology
Yu et al. An adaptive unscented particle filter algorithm through relative entropy for mobile robot self-localization
CN108681621B (en) RTS Kalman smoothing method based on Chebyshev orthogonal polynomial expansion
CN113670315B (en) Variation iterative Kalman filtering-based method for estimating attitude of Liqun heavy tail interference noise dynamic aircraft
Quang et al. Design a nonlinear MPC controller for autonomous mobile robot navigation system based on ROS
CN115933647B (en) OMR track tracking control method based on composite control algorithm and storage medium
CN111998854A (en) Cholesky decomposition calculation-based accurate expansion Stirling interpolation filtering method
Wang et al. Ground moving target tracking with quadrotors using nonlinear model predictive control
Gao et al. Improved 2D laser slam graph optimization based on Cholesky decomposition
Moore et al. Utilising fuzzy models in the design of estimators and predictors: An agile target tracking example

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230413

Address after: No. 3a20, incubation Building 1, No. 11, Changchun Road, high tech Industrial Development Zone, Zhengzhou, Henan 450000

Patentee after: Zhengzhou ousma Intelligent Control Technology Co.,Ltd.

Address before: 450002 No. 5 Dongfeng Road, Jinshui District, Henan, Zhengzhou

Patentee before: Zhengzhou University of light industry