CN115859039B - Vehicle state estimation method - Google Patents

Vehicle state estimation method Download PDF

Info

Publication number
CN115859039B
CN115859039B CN202310182028.0A CN202310182028A CN115859039B CN 115859039 B CN115859039 B CN 115859039B CN 202310182028 A CN202310182028 A CN 202310182028A CN 115859039 B CN115859039 B CN 115859039B
Authority
CN
China
Prior art keywords
representing
vehicle state
state estimation
kernel
equation
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
CN202310182028.0A
Other languages
Chinese (zh)
Other versions
CN115859039A (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.)
Nanjing University of Information Science and Technology
Original Assignee
Nanjing University of Information Science and Technology
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 Nanjing University of Information Science and Technology filed Critical Nanjing University of Information Science and Technology
Priority to CN202310182028.0A priority Critical patent/CN115859039B/en
Publication of CN115859039A publication Critical patent/CN115859039A/en
Application granted granted Critical
Publication of CN115859039B publication Critical patent/CN115859039B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Landscapes

  • Feedback Control In General (AREA)
  • Measurement Of Mechanical Vibrations Or Ultrasonic Waves (AREA)

Abstract

The invention discloses a vehicle state estimation method, which comprises the following steps: constructing a linear system of the vehicle state, wherein the linear system of the vehicle state is described by adopting a state equation and an observation equation, and noise in the state equation and the observation equation is described by adopting a non-Gaussian noise statistical model; under a linear system of the vehicle state, estimating the vehicle state by adopting a self-adaptive kernel maximum correlation entropy Kalman filtering method based on a mixed kernel function, so as to obtain an optimal state estimation of the vehicle state; the method comprises the steps of taking a weighted sum of a kernel function aiming at an observation equation residual term and a kernel function aiming at a state equation prediction error term as a cost function in a self-adaptive kernel maximum correlation entropy Kalman filtering method based on a mixed kernel function; and the kernel width of the kernel function is adaptively updated according to the residual term of the observation equation.

Description

Vehicle state estimation method
Technical Field
The invention belongs to the technical field of data processing, and particularly relates to a vehicle state estimation method.
Background
Conventional Kalman (Kalman) filtering and variants thereof, such as unscented Kalman filter UKF, extended Kalman filter EKF, volumetric Kalman filter CKF, etc., all use an algorithm based on a minimum Mean Square Error (MSE) criterion of an Error second moment as a cost function, and are widely used to realize optimal estimation under the assumption of linear systems and gaussian noise. However, the actual process noise and/or the measurement noise are far away from gaussian distribution due to artificial reasons, inaccurate modeling, unreliable equipment, sampling errors, network attack and the like, and in this case, kalman and variants thereof are adopted for filtering, so that larger deviation of an estimation result can occur and the optimization cannot be realized.
For this reason, in recent years, entropy (minimum error entropy MEE, maximum correlation entropy MCC, etc.) indexes based on error high-order moments are used as cost functions for filtering, and compared with Kalman filtering based on MSE indexes, the Kalman filtering precision, robustness, etc. of the entropy indexes are greatly improved. Since the computation complexity of Kalman filtering based on MEE indexes is much more complex than that based on MCC, the Kalman filtering based on MCC is more applied.
In MCC-based kalman filtering, the kernel width is the only free parameter, which plays a decisive role in the existence of local optimum, convergence speed, robustness to non-gaussian noise, and the like. However, most literature or actual engineering currently determines a core width of a selected fixed size based on empirical or trial and error methods for a particular non-gaussian noise. On the one hand, the non-Gaussian noise of an actual system is unknown, and the fixed-size kernel width determined based on a certain specific noise can have poor estimation performance under the condition of the actual non-Gaussian noise; on the other hand, the noise is not stable, such as the initial noise is large, and the noise tends to be stable along with the time, so that the adoption of the fixed kernel width is very easy to be less than optimal.
Disclosure of Invention
The invention aims to: in order to solve the problem that the estimation performance of a fixed-size kernel width determined based on a specific noise can be poor under the condition of actual non-Gaussian noise, and solve the problem that the adoption of the fixed kernel width is very easy to reach the optimal performance, the invention provides a vehicle state estimation method of a maximum correlation entropy Kalman filtering method based on a self-adaptive kernel, and aims at the condition that the actual system process noise and/or measurement noise is non-Gaussian, the filtering precision and the robustness are greatly improved, the performance of state estimation is improved, and the application range of the maximum correlation entropy Kalman filtering is greatly enhanced.
The technical scheme is as follows: a vehicle state estimation method, comprising the steps of:
step 1: constructing a linear system of vehicle states, wherein the linear system of vehicle states is described by adopting a state equation and an observation equation, and noise in the state equation and the observation equation is described by adopting a non-Gaussian noise statistical model; the vehicle state includes a vehicle position and a vehicle speed;
step 2: under a linear system of the vehicle state, estimating the vehicle state by adopting a self-adaptive kernel maximum correlation entropy Kalman filtering method based on a mixed kernel function, so as to obtain an optimal state estimation of the vehicle state;
the adaptive kernel maximum correlation entropy Kalman filtering method based on the mixed kernel function comprises the following steps:
according to the vehicle state estimation at the previous moment and the state estimation error covariance at the previous moment, performing one-step prediction to obtain the predicted vehicle state estimation and the prediction error covariance at the current moment;
taking the weighted sum of the kernel function for the observation equation residual term and the kernel function for the state equation prediction error term as a cost function; maximizing the cost function to obtain vehicle state estimation at the current moment and state estimation error covariance at the current moment;
the kernel widths of the kernel function for the observation equation residual term and the kernel function for the state equation prediction error term are adaptively updated according to the observation equation residual term.
Further, the method comprises the steps of,
the previous time is recorded as
Figure SMS_1
Time, current time is +.>
Figure SMS_2
Time;
the state equation is expressed as:
Figure SMS_3
（1）
in the method, in the process of the invention,
Figure SMS_5
representation->
Figure SMS_9
Vehicle state at time->
Figure SMS_13
Representation->
Figure SMS_6
State transition matrix of time->
Figure SMS_7
Representation->
Figure SMS_10
Vehicle state at time->
Figure SMS_12
Representation->
Figure SMS_4
Process noise at time; the process noise is obeyed to be 0 in mean value and covariance matrix is
Figure SMS_8
Is not gaussian, wherein +.>
Figure SMS_11
，/>
Figure SMS_14
Representing the desired operation, superscriptTRepresenting a transpose;
the observation equation is expressed as:
Figure SMS_15
（2）
in the method, in the process of the invention,
Figure SMS_17
representation->
Figure SMS_20
Observation output of time,/->
Figure SMS_22
Representation->
Figure SMS_18
Time observation matrix,/, for>
Figure SMS_19
Representation->
Figure SMS_21
Measuring noise at the moment; the measurement noise is subject to mean value 0 and covariance matrix of +>
Figure SMS_23
Is not gaussian, wherein,
Figure SMS_16
。
further, the noise in the state equation and the observation equation is described by a non-gaussian noise statistical model, which is specifically expressed as follows:
Figure SMS_24
（3）
Figure SMS_25
（4）
in the method, in the process of the invention,
Figure SMS_29
convex combining coefficients of gaussian components representing process noise and measurement noise respectively,
Figure SMS_33
non-gaussian intensity coefficients representing process noise and measurement noise, respectively, +.>
Figure SMS_37
Representing a coincidence mean of 0, variance +.>
Figure SMS_28
Normal distribution of->
Figure SMS_32
Representing a coincidence mean of 0, variance +.>
Figure SMS_36
Is used for the normal distribution of the (c),
Figure SMS_39
representing a coincidence mean of 0, variance +.>
Figure SMS_26
Normal distribution of->
Figure SMS_31
Representing a coincidence mean of 0 and variance of
Figure SMS_35
Normal distribution of->
Figure SMS_38
Representation->
Figure SMS_27
Process noise covariance of time of day,/>
Figure SMS_30
Representation->
Figure SMS_34
Measurement noise covariance of time.
Further, the step of predicting is performed according to the vehicle state estimation at the previous time and the state estimation error covariance at the previous time to obtain a predicted vehicle state estimation value and a predicted error covariance at the current time, specifically:
according to
Figure SMS_40
Vehicle state estimation and +.>
Figure SMS_41
The state estimation error co-formulation of the moment carries out one-step prediction according to the following prediction equation to obtain +.>
Figure SMS_42
Predicted vehicle state estimate and prediction error covariance at time: />
Figure SMS_43
（5）
Figure SMS_44
（6）
In the method, in the process of the invention,
Figure SMS_45
representation->
Figure SMS_46
Time-of-day one-step vehicle state prediction, +.>
Figure SMS_47
Representation->
Figure SMS_48
A vehicle state estimate of the time of day,
Figure SMS_49
representing a prediction error covariance; />
Figure SMS_50
Representation->
Figure SMS_51
The state of the moment estimates the error covariance.
Further, the observation equation residual term is expressed as:
Figure SMS_52
the method comprises the steps of carrying out a first treatment on the surface of the The state equation prediction error term is expressed as: />
Figure SMS_53
；
The kernel function for the observation equation residual term is expressed as:
Figure SMS_54
（7）
in the method, in the process of the invention,
Figure SMS_55
representing 2 norms>
Figure SMS_56
Representing 1 norm>
Figure SMS_57
Representing the mixing coefficient>
Figure SMS_58
The core width is indicated as being the number of cores,
Figure SMS_59
represents a square root function>
Figure SMS_60
Expressed in terms ofAn exponential function with a natural constant e as a base;
the kernel function for the state equation prediction error term is expressed as:
Figure SMS_61
（8）
the cost function is expressed as:
Figure SMS_62
（9）。
further, the kernel widths of the kernel function for the observation equation residual term and the kernel function for the state equation prediction error term are adaptively updated according to the observation equation residual term, and are expressed as follows:
Figure SMS_63
（10）
in the method, in the process of the invention,
Figure SMS_64
representation->
Figure SMS_65
Vehicle state estimation at time.
Further, the maximizing the cost function to obtain the vehicle state estimation at the current moment and the state estimation error covariance at the current moment specifically includes:
regarding cost function
Figure SMS_66
Vehicle state estimation +.>
Figure SMS_67
Derivative and let derivative be 0, expressed as:
Figure SMS_68
（11）
in the method, in the process of the invention,
Figure SMS_69
representing a sign function; />
Figure SMS_70
Abbreviations representing kernel functions for observation equation residual terms; />
Figure SMS_71
Abbreviations representing kernel functions for state equation prediction error terms; approximation in kernel function>
Figure SMS_72
；
The method comprises the following steps:
Figure SMS_73
（12）
Figure SMS_74
（13）
Figure SMS_75
（14）
in the method, in the process of the invention,
Figure SMS_76
representation->
Figure SMS_77
Dimension Unit matrix>
Figure SMS_78
Representing the filter gain +.>
Figure SMS_79
State estimation error covariance representing time of day, +.>
Figure SMS_80
Representing intermediate variables +.>
Figure SMS_81
。
The beneficial effects are that: compared with the prior art, the invention has the following advantages:
(1) Aiming at the problem that the process (sum) or measurement noise is actually non-Gaussian noise, the method adopts the related entropy index based on the error high-order moment to carry out Kalman filtering to reduce the interference of the non-Gaussian noise, can better extract the information in the error vector, improves the precision of state estimation and the robustness of the system, and achieves better filtering precision;
(2) The method is characterized based on non-Gaussian noise distribution and Gaussian homogeneous mixed distribution or heterogeneous mixture of Gaussian and other distribution mixtures, and the kernel function in the related entropy index is mixed with a heterogeneous (Gaussian kernel function plus exponential kernel function) kernel function to realize better filtering performance;
(3) Aiming at the actual situation that the actual system noise is not stable, the method adopts the self-adaptive kernel width to carry out the related entropy filtering, namely, based on the characteristic that the kernel width is the only free parameter in the maximum related entropy criterion (Maximum Correntropy Criterion, the MCC for short hereinafter), and plays a decisive role in filtering performance, the Gaussian kernel function in the MCC is analyzed, and the kernel width is self-adaptively updated along with the error term by adopting the weighted sum based on the residual term and the estimated error covariance as the kernel function in the MCC index; compared with the fixed kernel width, the method meets the characteristic of noise uncertainty in a state equation better, and achieves better filtering performance.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a graph of shot non-Gaussian noise;
FIG. 3 is a root mean square error for four state variables; fig. 3 (a) shows the root mean square error of the first state variable, fig. 3 (b) shows the root mean square error of the second state variable, fig. 3 (c) shows the root mean square error of the third state variable, and fig. 3 (d) shows the root mean square error of the fourth state variable;
FIG. 4 is a diagram of a pulsed non-Gaussian noise plot;
FIG. 5 is a root mean square error for four state variables; fig. 5 (a) shows the root mean square error of the first state variable, fig. 5 (b) shows the root mean square error of the second state variable, fig. 5 (c) shows the root mean square error of the third state variable, and fig. 5 (d) shows the root mean square error of the fourth state variable;
FIG. 6 is a dual Gaussian mixture non-Gaussian noise plot;
FIG. 7 is a root mean square error for four state variables; fig. 7 (a) shows the root mean square error of the first state variable, fig. 7 (b) shows the root mean square error of the second state variable, fig. 7 (c) shows the root mean square error of the third state variable, and fig. 7 (d) shows the root mean square error of the fourth state variable.
Detailed Description
The technical scheme of the invention is further described with reference to the accompanying drawings and the embodiments.
The traditional Kalman filtering based on the Mean Square Error (MSE) index reaches the optimal estimation under the assumption of the high-order gaussian noise, and when the noise in the actual state equation is the non-gaussian noise, the filtering performance of the traditional Kalman filter can be reduced.
The steps of this embodiment will now be further described with reference to fig. 1:
step 1: aiming at the vehicle navigation problem of the linear measurement model, a discrete time dynamic model of a linear system of vehicle states is constructed, wherein the discrete time dynamic model comprises a state equation and an observation equation:
Figure SMS_82
（1）
Figure SMS_83
（2）
in the method, in the process of the invention,
Figure SMS_100
representation->
Figure SMS_104
Vehicle state at time->
Figure SMS_107
And->
Figure SMS_86
Respectively indicate->
Figure SMS_88
Vehicle position and vehicle speed at time; />
Figure SMS_92
Representation->
Figure SMS_96
Vehicle state at time->
Figure SMS_102
Representation->
Figure SMS_106
Observing and outputting time; />
Figure SMS_110
And->
Figure SMS_113
Respectively indicate->
Figure SMS_103
State transition matrix and ∈time of day>
Figure SMS_108
An observation matrix of time; />
Figure SMS_111
And->
Figure SMS_114
Respectively indicate->
Figure SMS_87
Time process noise and measurement noise are uncorrelated; assuming that the process noise is subject to an average value of 0 and the covariance matrix is +.>
Figure SMS_89
Is subject to a mean value of 0 and a covariance matrix of +.>
Figure SMS_93
Is non-Gaussian distribution of (C) satisfying
Figure SMS_97
，/>
Figure SMS_84
，/>
Figure SMS_90
Representing the desired operation, superscriptTRepresenting transpose, < > due to unknown outliers and disturbances>
Figure SMS_94
Process noise covariance of time of day->
Figure SMS_98
And->
Figure SMS_85
Measurement noise covariance of time of day->
Figure SMS_91
Neither is accurate. />
Figure SMS_95
Is thatnReal number, < >>
Figure SMS_99
Is thatmReal number, < >>
Figure SMS_101
Is->
Figure SMS_105
Dimension real number matrix->
Figure SMS_109
Is->
Figure SMS_112
A matrix of dimensional real numbers.
For basic vehicle navigation problems, now taking four-dimensional state variables as an example, the first two state variables are the position coordinates of the north and east of the vehicle, and the second two state variables are the corresponding speeds, and therefore,
Figure SMS_115
and->
Figure SMS_116
Expressed as: />
Figure SMS_117
Figure SMS_118
Wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure SMS_119
representing the sampling interval.
Based on the statistical properties of non-gaussian noise in a discrete time dynamic model of a linear system, any non-gaussian distribution can be represented or approximated by the sum of a finite number of gaussian distributions. Here, the non-gaussian distributed process noise and the measurement noise are respectively represented by convex combinations of two gaussian components, so that a non-gaussian noise statistical model is constructed, as represented by formulas (3) and (4):
Figure SMS_120
（3）
Figure SMS_121
（4）
in the method, in the process of the invention,
Figure SMS_123
convex combining coefficients of gaussian components representing process noise and measurement noise respectively,
Figure SMS_126
the non-Gaussian intensity coefficients respectively representing the process noise and the measurement noise are stronger as the values are larger; />
Figure SMS_129
Representing a coincidence mean of 0, variance +.>
Figure SMS_124
Normal distribution of->
Figure SMS_125
Representing a coincidence mean of 0, variance +.>
Figure SMS_128
Normal distribution of->
Figure SMS_131
The representation accords with the mean value of 0 and squareDifference is->
Figure SMS_122
Normal distribution of->
Figure SMS_127
Representing a coincidence mean of 0, variance +.>
Figure SMS_130
Is a normal distribution of (c).
Step 2: based on the non-Gaussian noise statistical model in the step 1, the embodiment adopts a self-adaptive kernel maximum correlation entropy Kalman filtering method based on a mixed kernel function to obtain better filtering performance.
In order to better understand this step, a conventional kalman filtering process based on weighted least squares will now be described.
The traditional kalman filtering process based on weighted least squares is as follows:
aiming at the vehicle navigation problem of the linear measurement model, a discrete time dynamic model of a linear system of vehicle states is constructed, wherein the discrete time dynamic model comprises a state equation and an observation equation:
Figure SMS_132
（1）
Figure SMS_133
（2）
the state prediction phase includes performing one-step state prediction and one-step error covariance prediction, expressed as:
Figure SMS_134
（5）
Figure SMS_135
（6）
in the method, in the process of the invention,
Figure SMS_136
representation->
Figure SMS_137
Time-of-day one-step vehicle state prediction, +.>
Figure SMS_138
Representation->
Figure SMS_139
Vehicle state estimation at time ∈>
Figure SMS_140
Representing a prediction error covariance; />
Figure SMS_141
Representation->
Figure SMS_142
State estimation error covariance of time;
construction of cost function
Figure SMS_143
Expressed as:
Figure SMS_144
（15）
by solving for the accurate process noise covariance and measurement noise covariance when known
Figure SMS_145
A status update is obtained, expressed as:
Figure SMS_146
（16）
Figure SMS_147
（17）
Figure SMS_148
（18）
Figure SMS_149
（19）
in the method, in the process of the invention,
Figure SMS_151
representation->
Figure SMS_153
Vehicle state estimation at time ∈>
Figure SMS_156
、/>
Figure SMS_152
Respectively indicate->
Figure SMS_154
Time-of-day filter gain sum->
Figure SMS_157
State estimation error covariance of time instant +.>
Figure SMS_158
Representation->
Figure SMS_150
Dimension Unit matrix>
Figure SMS_155
Is the prediction error term.
The filtering process of the present embodiment also includes a state prediction phase and a state update phase.
Compared with the traditional Kalman filtering based on the weighted least squares, the state prediction stage of the embodiment is the same as the traditional Kalman filtering based on the weighted least squares, namely the state prediction stage of the embodiment is expressed as:
Figure SMS_159
（5）
Figure SMS_160
（6）
the state update stage of this embodiment replaces the weighted least squares based cost function in the traditional weighted least squares based kalman filter with a higher order moment based dependent entropy cost function. Specific:
taking the weighted sum of the kernel function for the observation equation residual term and the kernel function for the state equation prediction error term as a cost function; wherein, the residual term of the observation equation is expressed as:
Figure SMS_161
the method comprises the steps of carrying out a first treatment on the surface of the The state equation prediction error term is expressed as:
Figure SMS_162
；
the kernel function for the observation equation residual term is expressed as:
Figure SMS_163
（7）
in the method, in the process of the invention,
Figure SMS_164
representing 2 norms>
Figure SMS_165
Representing 1 norm>
Figure SMS_166
Representing the mixing coefficient>
Figure SMS_167
The core width is indicated as being the number of cores,
Figure SMS_168
represents a square root function>
Figure SMS_169
An exponential function based on a natural constant e;
the kernel function for the state equation prediction error term is expressed as:
Figure SMS_170
（8）
therefore, the cost function of the present embodiment is expressed as:
Figure SMS_171
（9）。
in other words, the present embodiment mixes the gaussian kernel function and the exponential kernel function by a mixed coefficient
Figure SMS_172
Blending as a higher order moment based dependent entropy cost function; gaussian kernel function, of general form: />
Figure SMS_173
The method comprises the steps of carrying out a first treatment on the surface of the An exponential kernel, of general form: />
Figure SMS_174
。
Whereas the kernel width is the only free parameter in the MCC, it directly determines the performance of the MCC based filter. Based on the analysis of the performance surface pattern of the kernel function, the present embodiment heuristically employs an adaptive kernel width, expressed as:
Figure SMS_175
（10）
in the method, in the process of the invention,
Figure SMS_176
representation->
Figure SMS_177
Vehicle state estimation at time.
As can be seen from equation (10), if disturbed by outliers or non-gaussian noise,
Figure SMS_178
nuclear width->
Figure SMS_179
Will be small and the filter will be effective in minimizing the correlation entropy whereas the kernel width will be larger when subjected to small disturbances.
Maximizing the cost function to obtain the vehicle state estimation at the current moment and the state estimation error covariance at the current moment, wherein the method specifically comprises the following steps:
to calculate
Figure SMS_180
Vehicle state estimation +.>
Figure SMS_181
I.e. solve->
Figure SMS_182
It is desirable to apply the formula (9) to +.>
Figure SMS_183
Vehicle state estimation +.>
Figure SMS_184
Derivative and let derivative be 0, namely:
Figure SMS_185
（11）
this embodiment utilizes approximations in the kernel function:
Figure SMS_186
。
in the method, in the process of the invention,
Figure SMS_187
abbreviations representing kernel functions for observation equation residual terms; />
Figure SMS_188
Shrinking of a kernel function representing a prediction error term for a state equationWriting; />
Figure SMS_189
Representing a sign function:
obtained by the formula (11):
Figure SMS_190
（20）/>
in the method, in the process of the invention,
Figure SMS_191
representing intermediate variables +.>
Figure SMS_192
；
Add to the right of (11) minus
Figure SMS_193
Combining to obtain:
Figure SMS_194
（12）
Figure SMS_195
（13）
in the method, in the process of the invention,
Figure SMS_196
is the filtering gain.
And then push out:
Figure SMS_197
（14）
in the method, in the process of the invention,
Figure SMS_198
、/>
Figure SMS_199
respectively represent the filter gain and->
Figure SMS_200
State estimation error covariance of time instant +.>
Figure SMS_201
Is->
Figure SMS_202
And (5) a dimensional identity matrix.
In summary, the state update phase of this embodiment is expressed as:
Figure SMS_203
（12）
Figure SMS_204
（13）
Figure SMS_205
（14）
the embodiment adopts the maximum correlation entropy (MCC) cost function based on the high-order moment of the error vector, so that the information in the error vector can be better extracted, and better filtering precision and robustness are achieved; based on the characteristic of non-Gaussian noise distribution, a heterogeneous kernel function based on Gaussian kernel plus exponential kernel mixing is adopted, so that better filtering performance can be realized; and the embodiment innovatively designs the fixed kernel width which is obtained based on trial and error to obtain the proper kernel width so as to obtain the good filtering performance as the self-adaptive kernel, and compared with the fixed kernel width, the method and the device are more in line with the characteristic of noise uncertainty in a state equation, and realize the better filtering performance.
To verify the filtering performance of the method of this embodiment, the method of this embodiment is compared with other filtering algorithms.
The basic navigation problem applied to linear uniform linear motion proposed in this embodiment is compared with the traditional Kalman Filter (KF) and the maximum correlation entropy Kalman filter (MCC-Maximum Correntropy Criterion Kalman filter) based on a single gaussian kernel function by adopting an adaptive kernel maximum correlation entropy Kalman filter method (Mix Kernel Maximum Correntropy Criterion Kalman filter, abbreviated as mk_mcc) based on a mixed kernel function (a combination of a gaussian kernel function and an exponential kernel function according to a certain coefficient), and a simulation result obtained by adopting Matlab R2018a is as follows:
for the basic navigation problem applied to linear motion at a uniform speed, taking four state variables as examples, the first two state variables are the position coordinates of the north and east of the vehicle, and the second two state variables are the corresponding speeds.
(1) The added noise is the shot non-gaussian noise shown in fig. 2, root Mean Square Error (RMSE) of four state variables under three filtering modes under the shot non-gaussian noise is shown in fig. 3, and table 1 shows estimation accuracy of three filtering methods under the shot non-gaussian noise.
Table 1 estimation accuracy of three filtering methods under shot non-gaussian noise
Figure SMS_206
In the table, x1 represents a first state variable, x2 represents a second state variable, x3 represents a third state variable, and x4 represents a fourth state variable.
Therefore, compared with the traditional Kalman filtering algorithm, the filtering method of the embodiment greatly improves the filtering performance under the condition of non-Gaussian noise shot, and verifies the superiority of the filtering method of the embodiment.
(2) The addition of the pulsed non-gaussian noise shown in fig. 4, under which the Root Mean Square Error (RMSE) of the four state variables in the three filtering modes is shown in fig. 5. Table 2 shows the estimated accuracy of the three filtering methods under pulsed non-gaussian noise.
Table 2 estimation accuracy of three filtering methods under pulsed non-gaussian noise
Figure SMS_207
Therefore, compared with the traditional Kalman filtering algorithm, the filtering method of the embodiment greatly improves the filtering performance under the condition of pulse non-Gaussian noise, and verifies the superiority of the filtering method of the embodiment.
(3) The non-Gaussian noise of the dual Gaussian mixture, as shown in fig. 6, was added, and the Root Mean Square Error (RMSE) of the four state variables in the three filtering modes at the non-Gaussian noise of the dual Gaussian mixture is shown in fig. 7. Table 3 shows the estimated accuracy of the three filtering methods under non-Gaussian noise of the dual Gaussian mixture.
TABLE 3 estimation accuracy of three filtering methods under double Gaussian mixture of non-Gaussian noise
Figure SMS_208
Therefore, compared with the traditional Kalman filtering algorithm, the filtering method of the embodiment greatly improves the filtering performance under the condition of double Gaussian mixture non-Gaussian noise, and verifies the superiority of the filtering method of the embodiment.

Claims (3)

1. A vehicle state estimation method, characterized by: the method comprises the following steps:
step 1: constructing a linear system of vehicle states, wherein the linear system of vehicle states is described by adopting a state equation and an observation equation, and noise in the state equation and the observation equation is described by adopting a non-Gaussian noise statistical model; the vehicle state includes a vehicle position and a vehicle speed;
step 2: under a linear system of the vehicle state, estimating the vehicle state by adopting a self-adaptive kernel maximum correlation entropy Kalman filtering method based on a mixed kernel function, so as to obtain an optimal state estimation of the vehicle state;
the adaptive kernel maximum correlation entropy Kalman filtering method based on the mixed kernel function comprises the following steps:
according to the vehicle state estimation at the previous moment and the state estimation error covariance at the previous moment, performing one-step prediction to obtain the predicted vehicle state estimation and the prediction error covariance at the current moment;
taking the weighted sum of the kernel function for the observation equation residual term and the kernel function for the state equation prediction error term as a cost function; maximizing the cost function to obtain vehicle state estimation at the current moment and state estimation error covariance at the current moment;
the kernel widths of the kernel function for the observation equation residual error term and the kernel function for the state equation prediction error term are adaptively updated according to the observation equation residual error term;
the previous moment is recorded as the k-1 moment, and the current moment is recorded as the k moment;
the state equation is expressed as:
x k ＝F k x k-1 +ω k (1)
wherein x is k Representing the vehicle state at time k, F k State transition matrix x representing time k k-1 Representing the vehicle state at time k-1, ω k Process noise at time k; the process noise is obeyed to be 0 in mean value and Q in covariance matrix k Is not gaussian, wherein,
Figure FDA0004161633940000012
e (·) represents the desired operation, and the superscript T represents the transpose;
the observation equation is expressed as:
y k ＝H k x k +v k (2)
wherein y is k Represents the observed output at time k, H k An observation matrix representing the moment k, v k Representing measurement noise at time k; the measurement noise is obeyed to be 0 in mean value and R in covariance matrix k Is not gaussian, wherein,
Figure FDA0004161633940000011
the noise in the state equation and the observation equation is described by adopting a non-Gaussian noise statistical model, and is specifically expressed as follows:
ω k ～(1-a)*N(0，Q k )+a*N(0，αQ k ) (3)
v k ～(1-b)*N(0，R k )+b*N(0，βR k ) (4)
wherein a and b are E (0, 1)]Convex combining coefficients of gaussian components representing process noise and measurement noise, respectively, alpha, beta e (0, ++ infinity) non-gaussian intensity coefficients, N (0, Q) k ) Representing a coincidence mean of 0 and a variance of Q k N (0, αq) k ) Representing a coincidence mean of 0 and variance αQ k Normal distribution of N (0, R) k ) Representing a coincidence mean of 0 and variance of R k N (0, βr) k ) Representing a mean value of 0 and a variance of βR k Normal distribution of Q k Represents the process noise covariance at time k, R k Representing the measurement noise covariance at time k;
the method comprises the steps of performing one-step prediction according to the vehicle state estimation at the previous moment and the state estimation error covariance at the previous moment to obtain a predicted vehicle state estimation value and a predicted error covariance at the current moment, wherein the specific steps are as follows:
according to the vehicle state estimation at the time of k-1 and the state estimation error covariance at the time of k-1, one-step prediction is carried out according to the following prediction equation, so as to obtain a predicted vehicle state estimation value and a predicted error covariance at the time of k:
Figure FDA0004161633940000021
P k|k-1 ＝F k P k-1 F k T +Q k (6)
in the method, in the process of the invention,
Figure FDA0004161633940000022
one-step vehicle state prediction representing time k, < ->
Figure FDA0004161633940000023
Vehicle state estimation, P, representing time k-1 k|k-1 Representing prediction error co-ordinatesVariance; p (P) k-1 Representing the state estimation error covariance at time k-1;
the observation equation residual term is expressed as: y is k -H k x k The method comprises the steps of carrying out a first treatment on the surface of the The state equation prediction error term is expressed as:
Figure FDA0004161633940000024
the kernel function for the observation equation residual term is expressed as:
Figure FDA0004161633940000025
in the formula, |·|| represents 2 norms, |·| represents 1 norms, |s [0,1] represents a mixing coefficient, σ > 0 represents a kernel width, sqrt (·) represents a square root function, exp (·) represents an exponential function based on a natural constant e;
the kernel function for the state equation prediction error term is expressed as:
Figure FDA0004161633940000031
the cost function is expressed as:
Figure FDA0004161633940000032
2. a vehicle state estimation method according to claim 1, characterized in that: the kernel widths of the kernel function for the observation equation residual term and the kernel function for the state equation prediction error term are adaptively updated according to the observation equation residual term, and are expressed as follows:
Figure FDA0004161633940000033
in the method, in the process of the invention,
Figure FDA0004161633940000034
the vehicle state estimate at time k is shown.
3. A vehicle state estimation method according to claim 2, characterized in that: the maximizing processing is carried out on the cost function to obtain the vehicle state estimation at the current moment and the state estimation error covariance at the current moment, specifically:
vehicle state estimation for cost function with respect to time k
Figure FDA0004161633940000035
Derivative and let derivative be 0, expressed as:
Figure FDA0004161633940000036
where sgn (·) represents a sign function; g σr Abbreviations representing kernel functions for observation equation residual terms; g σp Abbreviations representing kernel functions for state equation prediction error terms; approximation in kernel function
Figure FDA0004161633940000037
The method comprises the following steps:
Figure FDA0004161633940000038
/>
Figure FDA0004161633940000039
Figure FDA0004161633940000041
wherein I is n Represents an n-dimensional identity matrix, K MKC Representing the filter gain, P k|k Representing the state estimation error covariance at time k, L k Represents an intermediate variable which is referred to as,
Figure FDA0004161633940000042
/>
CN202310182028.0A 2023-03-01 2023-03-01 Vehicle state estimation method Active CN115859039B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310182028.0A CN115859039B (en) 2023-03-01 2023-03-01 Vehicle state estimation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310182028.0A CN115859039B (en) 2023-03-01 2023-03-01 Vehicle state estimation method

Publications (2)

Publication Number Publication Date
CN115859039A CN115859039A (en) 2023-03-28
CN115859039B true CN115859039B (en) 2023-05-23

Family

ID=85659442

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310182028.0A Active CN115859039B (en) 2023-03-01 2023-03-01 Vehicle state estimation method

Country Status (1)

Country Link
CN (1) CN115859039B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117713750B (en) * 2023-12-14 2024-05-17 河海大学 Consistency Kalman filtering state estimation method based on fractional power

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6677533B2 (en) * 2016-03-01 2020-04-08 クラリオン株式会社 In-vehicle device and estimation method
CN108983215A (en) * 2018-05-25 2018-12-11 哈尔滨工程大学 A kind of method for tracking target based on maximum cross-correlation entropy adaptively without mark particle filter
CN109084767A (en) * 2018-06-15 2018-12-25 哈尔滨工程大学 A kind of AUV collaborative navigation method of the adaptive volume particle filter of maximum cross-correlation entropy
CN112115419B (en) * 2020-09-14 2024-07-12 深圳大学 System state estimation method and system state estimation device
CN113449384A (en) * 2021-07-07 2021-09-28 中国人民解放军军事科学院国防科技创新研究院 Attitude determination method based on central error entropy criterion extended Kalman filtering
CN114880874B (en) * 2022-06-07 2024-03-12 东南大学 Self-adaptive robust estimation method and system for parameters of unmanned ship on water surface

Also Published As

Publication number Publication date
CN115859039A (en) 2023-03-28

Similar Documents

Publication Publication Date Title
Jazwinski Limited memory optimal filtering
Dogancay Bias compensation for the bearings-only pseudolinear target track estimator
Farahmand et al. Doubly robust smoothing of dynamical processes via outlier sparsity constraints
CN115859039B (en) Vehicle state estimation method
CN110501696A (en) A kind of radar target tracking method based on Doppler measurements self-adaptive processing
Tseng et al. Robust Huber-based cubature Kalman filter for GPS navigation processing
CN116595897B (en) Nonlinear dynamic system state estimation method and device based on message passing
Caballero-Aguila et al. Extended and unscented filtering algorithms in nonlinear fractional order systems with uncertain observations
Zhang et al. Gaussian approximate filter for stochastic dynamic systems with randomly delayed measurements and colored measurement noises
CN117614789B (en) Carrier phase tracking method and device based on Kalman-like unbiased FIR filter
CN109341690B (en) Robust and efficient combined navigation self-adaptive data fusion method
Tekalp et al. Fast recursive estimation of the parameters of a space-varying autoregressive image model
Ra et al. Recursive robust least squares estimator for time-varying linear systems with a noise corrupted measurement matrix
Wang et al. Accurate smoothing methods for state estimation of continuous-discrete nonlinear dynamic systems
CN116577750A (en) Kalman filtering algorithm based on intermediate transition state
Bodenschatz et al. Maximum-likelihood symmetric/spl alpha/-stable parameter estimation
CN114445459B (en) Continuous-discrete maximum correlation entropy target tracking method based on variable decibel leaf theory
CN113030945B (en) Phased array radar target tracking method based on linear sequential filtering
Guan et al. Optimal step size of least mean absolute third algorithm
CN114611068A (en) High maneuvering target tracking method
Poore et al. Batch maximum likelihood (ML) and maximum a posteriori (MAP) estimation with process noise for tracking applications
Ali et al. Convergence analysis of unscented transform for underwater passive target tracking in noisy environment
Huber et al. Efficient nonlinear measurement updating based on Gaussian mixture approximation of conditional densities
Kawano et al. A fundamental performance limit of cloud-based control in terms of differential privacy level
Abdelkrim et al. A simplified αβ based Gaussian sum filter

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