CN115859039A - Vehicle state estimation method - Google Patents

Vehicle state estimation method Download PDF

Info

Publication number
CN115859039A
CN115859039A CN202310182028.0A CN202310182028A CN115859039A CN 115859039 A CN115859039 A CN 115859039A CN 202310182028 A CN202310182028 A CN 202310182028A CN 115859039 A CN115859039 A CN 115859039A
Authority
CN
China
Prior art keywords
vehicle state
equation
state estimation
kernel
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.)
Granted
Application number
CN202310182028.0A
Other languages
Chinese (zh)
Other versions
CN115859039B (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

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 the 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 the optimal state estimation of the vehicle state; taking a weighted sum of a kernel function aiming at an observation equation residual error item and a kernel function aiming at a state equation prediction error item as a cost function in the adaptive kernel maximum correlation entropy Kalman filtering method based on the mixed kernel function; and the kernel width of the kernel function is adaptively updated according to the residual error 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
Traditional Kalman filtering and its variants such as unscented Kalman filtering UKF, extended Kalman filtering EKF, volumetric Kalman filtering CKF, etc. all use algorithms with a cost function based on the Mean-Square Error (MSE) criterion of the second order of Error, achieving optimal estimation under the assumption of linear system and gaussian noise, and are widely used. However, the actual process noise and/or measurement noise are far away from gaussian distribution due to human factors, inaccurate modeling, unreliable equipment, sampling errors, network attack and the like, and in this case, filtering is performed by using Kalman and its variants, and the estimation result has large deviation and cannot be optimal.
Therefore, in recent years, entropy (minimum error entropy MEE, maximum correlation entropy MCC, etc.) indexes based on high order moments of errors are used as cost functions for filtering, and compared with Kalman filtering based on MSE indexes, kalman filtering precision, robustness, etc. of the entropy indexes are greatly improved. Because the computation complexity of Kalman filtering based on MEE index is much more complicated and the computation amount is large than that based on MCC, the Kalman filtering based on MCC is more applied.
In the MCC-based kalman filtering, kernel width is a unique free parameter, and plays a decisive role in the existence of a local optimum value, convergence speed, robustness to non-gaussian noise, and the like. However, most existing literature or actual engineering determines to select a kernel width of a fixed size based on experience or trial and error methods for a certain non-gaussian noise. On one hand, the non-Gaussian noise of the actual system is unknown, and the estimation performance of the fixed kernel width determined based on a certain specific noise under the actual non-Gaussian noise condition may be poor; on the other hand, the noise is not stable, for example, the initial noise is very large, and the noise tends to be stable over time, so that the fixed kernel width is very easy to be less than optimal.
Disclosure of Invention
The purpose of the invention is as follows: aiming at solving the problems that the estimation performance of the fixed kernel width determined based on certain specific noise is possibly poor under the actual non-Gaussian noise condition and the fixed kernel width is very easy to be not optimal, the invention provides the vehicle state estimation method based on the maximum correlation entropy Kalman filtering method of the self-adaptive kernel, aiming at the condition that the noise in the actual system process and/or the measured noise is non-Gaussian, the large improvement of the filtering precision and the robustness is realized, the performance improvement of the state estimation is realized, and the application range of the maximum correlation entropy Kalman filtering is greatly strengthened.
The technical scheme is as follows: a vehicle state estimation method, comprising the steps of:
step 1: 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; the vehicle state includes a vehicle position and a vehicle speed;
and 2, step: 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 the 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:
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 the predicted vehicle state estimation and the predicted error covariance at the current moment;
taking the weighted sum of the kernel function for the residual error term of the observation equation and the kernel function for the prediction error term of the state equation as a cost function; performing maximization processing on the cost function to obtain the vehicle state estimation at the current moment and the state estimation error covariance at the current moment;
and the kernel widths of the kernel function aiming at the residual error term of the observation equation and the kernel function aiming at the prediction error term of the state equation are adaptively updated according to the residual error term of the observation equation.
Further, in the above-mentioned case,
remembering the previous moment as
Figure SMS_1
At a moment in time, the current moment in time is->
Figure SMS_2
Time of day;
the equation of state is expressed as:
Figure SMS_3
（1）
in the formula (I), the compound is shown in the specification,
Figure SMS_5
represents->
Figure SMS_9
The vehicle state at that moment, based on the status of the vehicle>
Figure SMS_13
Represents->
Figure SMS_6
The state transition matrix of the instant>
Figure SMS_7
Represents->
Figure SMS_10
The vehicle state at that moment, based on the status of the vehicle>
Figure SMS_12
Represents->
Figure SMS_4
Process noise at the time; the process noise is obeyed to mean 0 and covariance matrix @>
Figure SMS_8
In which it is not Gaussian, wherein>
Figure SMS_11
，/>
Figure SMS_14
Indicating desired operation, superscriptTRepresenting a transpose;
the observation equation is expressed as:
Figure SMS_15
（2）
in the formula (I), the compound is shown in the specification,
Figure SMS_17
represents->
Figure SMS_20
Observed output at that moment, is asserted>
Figure SMS_22
Represents->
Figure SMS_18
The observation matrix of the moment, ->
Figure SMS_19
Represents->
Figure SMS_21
Measurement noise at a time; the measurement noise is obeyed to mean 0 and covariance matrix is->
Figure SMS_23
In a non-Gaussian distribution of (a), wherein>
Figure SMS_16
。
Further, the noise in the state equation and the observation equation is described by using a non-gaussian noise statistical model, which is specifically expressed as:
Figure SMS_24
（3）
Figure SMS_25
（4）
in the formula (I), the compound is shown in the specification,
Figure SMS_29
convex combination 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
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_28
Is normally distributed over>
Figure SMS_32
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_36
Is normally distributed over>
Figure SMS_39
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_26
Is normally distributed over>
Figure SMS_31
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_35
Is normally distributed over>
Figure SMS_38
Represents->
Figure SMS_27
The process noise covariance of the moment @>
Figure SMS_30
Represents->
Figure SMS_34
The measured noise covariance of the time instants.
Further, the further prediction is performed according to the vehicle state estimation at the previous time and the state estimation error covariance at the previous time to obtain the predicted vehicle state estimation value and the predicted error covariance at the current time, and specifically:
according to
Figure SMS_40
Vehicle state estimation and->
Figure SMS_41
And performing one-step prediction on the state estimation error covariance at the moment according to the following prediction equation to obtain->
Figure SMS_42
Predicted vehicle state estimation value and prediction error covariance at time:
Figure SMS_43
（5）
Figure SMS_44
（6）
in the formula (I), the compound is shown in the specification,
Figure SMS_45
represents->
Figure SMS_46
One-step vehicle state prediction at a time, based on a time instant>
Figure SMS_47
Represents->
Figure SMS_48
The estimation of the state of the vehicle at the moment,
Figure SMS_49
representing the prediction error covariance; />
Figure SMS_50
Represents->
Figure SMS_51
State estimation error covariance at time. />
Further, the observation equation residual term is expressed as:
Figure SMS_52
(ii) a The equation of state prediction error term is expressed as: />
Figure SMS_53
；
The kernel function for the residual term of the observation equation is expressed as:
Figure SMS_54
（7）
in the formula (I), the compound is shown in the specification,
Figure SMS_55
represents a 2 norm,. Sup.>
Figure SMS_56
Represents a 1 norm,. Sup.>
Figure SMS_57
Represents a mixing factor, <' > is selected>
Figure SMS_58
Indicates that the nucleus is wide and/or is selected>
Figure SMS_59
Represents a square root function, <' > based on the square root function>
Figure SMS_60
Expressing an exponential function with a natural constant e as a base;
the kernel function for the prediction error term of the state equation is expressed as:
Figure SMS_61
（8）
the cost function is represented 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:
Figure SMS_63
（10）
in the formula (I), the compound is shown in the specification,
Figure SMS_64
represents->
Figure SMS_65
Vehicle state estimation at time.
Further, the maximizing the cost function to obtain the vehicle state estimation at the current time and the covariance of the state estimation error at the current time specifically includes:
to cost function about
Figure SMS_66
Vehicle state estimate of a time instant->
Figure SMS_67
Derivative and let the derivative be 0, expressed as:
Figure SMS_68
（11）
in the formula (I), the compound is shown in the specification,
Figure SMS_69
representing a symbolic function; />
Figure SMS_70
Abbreviations representing kernel functions for the observation equation residual terms; />
Figure SMS_71
Abbreviations representing kernel functions for the state equation prediction error terms; making an approximation in the kernel function>
Figure SMS_72
；
Obtaining:
Figure SMS_73
（12）
Figure SMS_74
（13）
Figure SMS_75
（14）/>
in the formula (I), the compound is shown in the specification,
Figure SMS_76
represents->
Figure SMS_77
Dimension unit matrix,. Or>
Figure SMS_78
Represents the filter gain, <' > is greater than>
Figure SMS_79
State estimation error covariance, representing time of day, is greater than>
Figure SMS_80
Represents an intermediate variable, <' > based on>
Figure SMS_81
。
Has the beneficial effects that: compared with the prior art, the invention has the following advantages:
(1) Aiming at the problem that the process (sum) or the measured noise is actually non-Gaussian noise, the method adopts the relevant 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 an error vector, improves the precision of state estimation and the robustness of a system, and achieves better filtering precision;
(2) The method is characterized by comprising the steps of describing a non-Gaussian noise distribution multi-base Gaussian homogeneous mixed distribution or heterogeneous mixture of Gaussian and other distributions, and mixing kernel functions of kernel functions in related entropy indexes with heterogeneous (Gaussian kernel functions and exponential kernel functions) to realize better filtering performance;
(3) Aiming at the actual situation that the noise of an actual system is not stable, the method adopts a self-adaptive kernel width to carry out correlation entropy filtering, namely, based on the characteristic that the kernel width is the only free parameter in the Maximum correlation entropy Criterion (MCC for short), and plays a decisive role in the filtering performance, analyzes Gaussian kernel functions in the MCC, adopts a weighted sum based on a residual error item and an estimated error covariance as a kernel function in the MCC index, and adaptively updates the kernel width along with the error item; compared with the fixed kernel width, the method is more consistent with the characteristic of noise uncertainty in a state equation, and better filtering performance is realized.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a shot non-Gaussian noise plot;
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, (b) in fig. 3 shows the root mean square error of the second state variable, (c) in fig. 3 shows the root mean square error of the third state variable, and (d) in fig. 3 shows the root mean square error of the fourth state variable;
FIG. 4 is a graph of impulse non-Gaussian noise;
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, (b) of fig. 5 shows the root mean square error of the second state variable, (c) of fig. 5 shows the root mean square error of the third state variable, and (d) of fig. 5 shows the root mean square error of the fourth state variable;
FIG. 6 is a double 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, (b) of fig. 7 shows the root mean square error of the second state variable, (c) of fig. 7 shows the root mean square error of the third state variable, and (d) of fig. 7 shows the root mean square error of the fourth state variable.
Detailed Description
The technical solution of the present invention will be further explained with reference to the accompanying drawings and examples.
In the embodiment, for any actual condition of process noise and/or measurement noise being non-Gaussian noise, actual condition of process noise being non-Gaussian noise, and actual condition of process noise being non-Gaussian noise, or actual condition of measurement noise being non-Gaussian noise, based on statistical characteristics of non-Gaussian noise, the cost function index is changed to adopt a related entropy index based on high order moment of error signal, and a single kernel Gaussian function in the index is changed to a mixed kernel function formed by combining a Gaussian kernel function and an exponential kernel function according to a certain coefficient, the mixed kernel function can extract more information in non-Gaussian error signal, the kernel width is a Maximum entropy related Criterion (Maximum reinitiation Criterion, hereinafter referred to as Critison), the only free parameter in the index and plays a role in the filter performance, the filter performance is changed based on a wide weighted kernel function, and the filter kernel function is used as a new observation kernel function, and the filter performance index is changed based on a wide observation kernel function, and a wide residual error adaptive residual error analysis, and the characteristics of the MCC are adopted to improve the filter performance of the filter performance.
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 the vehicle state is constructed, and the discrete time dynamic model comprises a state equation and an observation equation:
Figure SMS_82
（1）
Figure SMS_83
（2）
in the formula (I), the compound is shown in the specification,
Figure SMS_100
represents->
Figure SMS_104
Vehicle state at a moment in time>
Figure SMS_107
And &>
Figure SMS_86
Respectively represent->
Figure SMS_88
Vehicle position and vehicle speed at the time; />
Figure SMS_92
Represents->
Figure SMS_96
The vehicle state at that moment, based on the status of the vehicle>
Figure SMS_102
Represents->
Figure SMS_106
The observation output of the moment; />
Figure SMS_110
And
Figure SMS_113
respectively represent->
Figure SMS_103
The state transition matrix and of the instant>
Figure SMS_108
An observation matrix of the time; />
Figure SMS_111
And &>
Figure SMS_114
Respectively represent->
Figure SMS_87
Temporal process noise and measurement noise, which are uncorrelated; assume process noise as obeying a mean of 0 and a covariance matrix of>
Figure SMS_89
Non-gaussian distribution of (1), the measurement noise being subject to a mean value of0. Covariance matrix of ≥ v>
Figure SMS_93
Is not gaussian, satisfies->
Figure SMS_97
，
Figure SMS_84
，/>
Figure SMS_90
Indicating desired operation, superscriptTRepresents transposition, due to unknown outliers and interferences, is evaluated>
Figure SMS_94
Process noise covariance of a time instant ≥>
Figure SMS_98
And &>
Figure SMS_85
The measurement noise covariance of the moment ≥ is ≥>
Figure SMS_91
Are not precise. />
Figure SMS_95
Is composed ofnDimension and number->
Figure SMS_99
Is composed ofmDimension and number->
Figure SMS_101
Is->
Figure SMS_105
Dimension real number matrix, <' > based on>
Figure SMS_109
Is->
Figure SMS_112
A matrix of real numbers is maintained.
To basicThe vehicle navigation problem, now exemplified by four-dimensional state variables, the first two state variables are the position coordinates of the north and east of the vehicle, and the last two state variables are the corresponding speeds, and therefore,
Figure SMS_115
and &>
Figure SMS_116
Respectively expressed as:
Figure SMS_117
Figure SMS_118
wherein the content of the first and second substances,
Figure SMS_119
indicating the sampling interval.
Based on the statistical properties of non-gaussian noise in a discrete-time dynamical model of a linear system, any non-gaussian distribution can be represented or approximated by the sum of a finite number of gaussian distributions. The non-Gaussian distributed process noise and the measurement noise are respectively expressed by convex combination of two Gaussian components, so that a non-Gaussian noise statistical model is constructed and obtained, and the non-Gaussian noise statistical model is expressed by formulas (3) and (4):
Figure SMS_120
（3）
Figure SMS_121
（4）
in the formula (I), the compound is shown in the specification,
Figure SMS_123
convex combination coefficients of Gaussian components representing process noise and measurement noise, respectively, <>
Figure SMS_126
non-Gaussian intensity coefficients respectively representing process noise and measurement noise, wherein the larger the value of the non-Gaussian intensity coefficient is, the stronger the non-Gaussian degree is; />
Figure SMS_129
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_124
Is normally distributed over>
Figure SMS_125
Representing a mean of coincidence of 0 and a variance of
Figure SMS_128
Is normally distributed over>
Figure SMS_131
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_122
Is normally distributed over>
Figure SMS_127
Means that the mean of coincidence is 0 and the variance is->
Figure SMS_130
Is normally distributed.
Step 2: based on the non-gaussian noise statistical model in step 1, the present embodiment employs an 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, the conventional kalman filtering process based on weighted least squares is explained as follows.
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 the vehicle state is constructed, and the discrete time dynamic model comprises a state equation and an observation equation:
Figure SMS_132
（1）
Figure SMS_133
（2）
the state prediction stage includes performing a one-step state prediction and a one-step error covariance prediction, expressed as:
Figure SMS_134
（5）
Figure SMS_135
（6）
in the formula (I), the compound is shown in the specification,
Figure SMS_136
represents->
Figure SMS_137
One-step vehicle state prediction at a time, based on a time instant>
Figure SMS_138
Represents->
Figure SMS_139
The estimation of the state of the vehicle at the moment,
Figure SMS_140
representing the prediction error covariance; />
Figure SMS_141
Represents->
Figure SMS_142
State estimation error covariance at the moment;
constructing a cost function
Figure SMS_143
Expressed as:
Figure SMS_144
（15）
by solving for accurate process noise covariance and measured noise covariance when known
Figure SMS_145
A status update is obtained, denoted as:
Figure SMS_146
（16）
Figure SMS_147
（17）
Figure SMS_148
（18）
Figure SMS_149
（19）
in the formula (I), the compound is shown in the specification,
Figure SMS_151
represents->
Figure SMS_153
Vehicle state estimation of a time instant>
Figure SMS_156
、/>
Figure SMS_152
Respectively represent->
Figure SMS_154
The sum of the filter gains at a time instant & ->
Figure SMS_157
State estimate error covariance at a time instant>
Figure SMS_158
Represents->
Figure SMS_150
Dimension unit matrix,. Or>
Figure SMS_155
Is a prediction error term.
The filtering process of this embodiment also includes a state prediction stage and a state update stage.
Compared with the traditional kalman filtering based on weighted least squares, the state prediction phase of the present embodiment is the same as that of the traditional kalman filtering based on weighted least squares, that is, the state prediction phase of the present embodiment is expressed as:
Figure SMS_159
（5）
Figure SMS_160
（6）
in the state updating stage of the embodiment, a correlation entropy cost function based on a high-order moment is adopted to replace a cost function based on a weighted least square generation in the traditional kalman filtering based on the weighted least square. Specifically, the method comprises the following steps:
taking the weighted sum of the kernel function for the residual error term of the observation equation and the kernel function for the prediction error term of the state equation as a cost function; wherein, the observation equation residual term is expressed as:
Figure SMS_161
(ii) a The equation of state prediction error term is expressed as:
Figure SMS_162
；
the kernel function for the residual term of the observation equation is expressed as:
Figure SMS_163
（7）
in the formula (I), the compound is shown in the specification,
Figure SMS_164
represents a 2 norm,. Sup.>
Figure SMS_165
Represents a 1 norm,. Sup.>
Figure SMS_166
Represents a mixing factor, <' > is selected>
Figure SMS_167
Indicates kernel width +>
Figure SMS_168
Represents a square root function, <' > based on the square root function>
Figure SMS_169
Expressing an 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_170
（8）
therefore, the cost function of this embodiment is represented as:
Figure SMS_171
（9）。
in other words, the present embodiment mixes the gaussian kernel function and the exponential kernel function by the coefficient
Figure SMS_172
Mixing as a related entropy cost function based on high-order moments; a gaussian kernel function, generally of the form: />
Figure SMS_173
(ii) a An exponential kernel function, generally of the form: />
Figure SMS_174
。
Since kernel width is the only free parameter in MCC, it directly determines the performance of MCC based filters. Based on the analysis of the performance surface graph of the kernel function, the present embodiment heuristically employs an adaptive kernel width, which is expressed as:
Figure SMS_175
（10）
in the formula (I), the compound is shown in the specification,
Figure SMS_176
represents->
Figure SMS_177
Vehicle state estimation at time.
As can be seen from the equation (10), when the interference of abnormal value or non-Gaussian noise is received,
Figure SMS_178
nucleus wide->
Figure SMS_179
It will be small and the filter will effectively minimize the associated entropy whereas the kernel width is large when subjected to small disturbances.
The cost function is maximized to obtain the vehicle state estimation at the current moment and the covariance of the state estimation error at the current moment, which specifically comprises the following steps:
to seek to
Figure SMS_180
Vehicle state estimate of a time instant->
Figure SMS_181
I.e. solve for>
Figure SMS_182
The value of the pair of formula (9) is required to be greater than->
Figure SMS_183
Vehicle state estimate of a time instant->
Figure SMS_184
Derivative and let the derivative be 0, i.e.:
Figure SMS_185
（11）
this embodiment utilizes an approximation in the kernel function:
Figure SMS_186
。
in the formula (I), the compound is shown in the specification,
Figure SMS_187
abbreviations representing kernel functions for the observation equation residual terms; />
Figure SMS_188
Abbreviations representing kernel functions for the state equation prediction error terms; />
Figure SMS_189
Represents the symbolic function:
obtained by the formula (11):
Figure SMS_190
（20）
in the formula (I), the compound is shown in the specification,
Figure SMS_191
represents an intermediate variable, <' > is selected>
Figure SMS_192
；
Add and subtract to the right side of equation (11)
Figure SMS_193
And combining to obtain:
Figure SMS_194
（12）
Figure SMS_195
（13）
in the formula (I), the compound is shown in the specification,
Figure SMS_196
is the filter gain.
And then, the following steps are obtained:
Figure SMS_197
（14）
in the formula (I), the compound is shown in the specification,
Figure SMS_198
、/>
Figure SMS_199
respectively representing the filter gain and->
Figure SMS_200
State estimation error covariance at time @>
Figure SMS_201
Is->
Figure SMS_202
A dimension unit matrix.
To sum up, the state updating phase of the embodiment is represented as:
Figure SMS_203
（12）
Figure SMS_204
（13）
Figure SMS_205
（14）
according to the method, the maximum correlation entropy (MCC) cost function based on the high-order moment of the error vector is adopted, so that information in the error vector can be better extracted, and better filtering precision and robustness are achieved; based on the non-Gaussian noise distribution characteristic, a heterogeneous kernel function based on Gaussian kernel plus exponential kernel mixing is adopted, so that better filtering performance can be realized; in addition, the fixed kernel width that is obtained based on trial and error to obtain a proper kernel width to obtain good filtering performance is innovatively designed as an adaptive kernel, and compared with the fixed kernel width, the adaptive kernel more conforms to the characteristic of noise uncertainty in a state equation, so that better filtering performance is realized.
In order to verify the filtering performance of the method of the embodiment, the method of the embodiment is compared with other filtering algorithms.
Comparing the basic navigation problem applied to linear uniform linear motion proposed by this embodiment with a self-adaptive Kernel Maximum correlation entropy Kalman filter (Mix Kernel Maximum entropy Criterion Kalman filter, abbreviated as MK _ MCC) based on a mixed Kernel function (a gaussian Kernel function and an exponential Kernel function are combined according to a certain coefficient), a traditional Kalman Filter (KF) and a Maximum correlation entropy Kalman filter (MCC-Maximum entropy Criterion Kalman filter) based on a single gaussian Kernel function, and simulation results obtained by using Matlab R2018a are as follows:
for the basic navigation problem applied to linear uniform linear motion, four state variables are taken as an example, the first two state variables are position coordinates of the north part and the east part of the vehicle, and the last two state variables are corresponding speeds.
(1) The added noise is shot non-gaussian noise shown in fig. 2, the Root Mean Square Error (RMSE) of four state variables under three filtering modes is shown in fig. 3 under the shot non-gaussian noise, and table 1 shows the estimation accuracy of the 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 denotes a first state variable, x2 denotes a second state variable, x3 denotes a third state variable, and x4 denotes 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 shot non-Gaussian noise, and the superiority of the filtering method of the embodiment is verified.
(2) With the addition of the impulse non-gaussian noise as shown in fig. 4, the Root Mean Square Error (RMSE) of the four state variables under the three filtering modes is shown in fig. 5. Table 2 shows the estimation accuracy of the three filtering methods under pulsed non-gaussian noise.
TABLE 2 estimation accuracy of three filtering methods under impulse non-Gaussian noise
Figure SMS_207
Therefore, under the pulse non-Gaussian noise, compared with the traditional Kalman filtering algorithm, the filtering method of the embodiment greatly improves the filtering performance and verifies the superiority of the filtering method of the embodiment.
(3) With the addition of the double-Gaussian mixed non-Gaussian noise as shown in fig. 6, the Root Mean Square Error (RMSE) of the four state variables under the three filtering modes is shown in fig. 7. Table 3 shows the estimation accuracy of the three filtering methods under the double Gaussian mixed non-Gaussian noise.
TABLE 3 estimation accuracy of three filtering methods under double Gaussian mixed 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 and verifies the superiority of the filtering method of the embodiment under the condition of double-Gaussian mixed non-Gaussian noise.

Claims (7)

1. A vehicle state estimation method characterized by: the method comprises the following steps:
step 1: 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; the vehicle state includes a vehicle position and a vehicle speed;
and 2, step: 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 the 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:
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 the predicted vehicle state estimation and the predicted error covariance at the current moment;
taking the weighted sum of the kernel function for the residual error term of the observation equation and the kernel function for the prediction error term of the state equation as a cost function; carrying out maximization processing on the cost function to obtain vehicle state estimation at the current moment and state estimation error covariance at the current moment;
and the kernel widths of the kernel function aiming at the residual error term of the observation equation and the kernel function aiming at the prediction error term of the state equation are adaptively updated according to the residual error term of the observation equation.
2. A vehicle state estimation method according to claim 1, characterized in that:
remembering the previous moment as
Figure QLYQS_1
At a moment in time, the current moment in time is->
Figure QLYQS_2
Time of day;
the equation of state is expressed as:
Figure QLYQS_3
（1）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_5
represents->
Figure QLYQS_9
The vehicle state at that moment, based on the status of the vehicle>
Figure QLYQS_12
Represents->
Figure QLYQS_4
The state transition matrix of the instant>
Figure QLYQS_8
Represents->
Figure QLYQS_11
The vehicle state at that moment, based on the status of the vehicle>
Figure QLYQS_14
Represents->
Figure QLYQS_6
Process noise at the time; the process noise is obeyed to mean 0 and covariance matrix @>
Figure QLYQS_7
In which it is not Gaussian, wherein>
Figure QLYQS_10
，/>
Figure QLYQS_13
Indicating desired operation, superscriptTRepresenting a transpose;
the observation equation is expressed as:
Figure QLYQS_15
（2）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_17
represents->
Figure QLYQS_19
Observed output at that moment, is asserted>
Figure QLYQS_21
Represents->
Figure QLYQS_18
The observation matrix of the moment, ->
Figure QLYQS_20
Represents->
Figure QLYQS_22
Measurement noise at a time; the measurement noise is obeyed to mean 0 and covariance matrix is->
Figure QLYQS_23
In which it is not Gaussian, wherein>
Figure QLYQS_16
。
3. A vehicle state estimation method according to claim 2, characterized in that: 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:
Figure QLYQS_24
（3）
Figure QLYQS_25
（4）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_28
of gaussian components representing process noise and measurement noise, respectivelyThe coefficient of the convex combination is,
Figure QLYQS_31
non-Gaussian intensity coefficients representing process noise and measurement noise, respectively>
Figure QLYQS_35
Means coincidence mean 0 and variance +>
Figure QLYQS_29
Is normally distributed over>
Figure QLYQS_33
Means that the mean of coincidence is 0 and the variance is->
Figure QLYQS_37
Is normally distributed over>
Figure QLYQS_39
Means that the mean of coincidence is 0 and the variance is->
Figure QLYQS_26
Is normally distributed over>
Figure QLYQS_30
Means that the mean of coincidence is 0 and the variance is->
Figure QLYQS_34
Is normally distributed over>
Figure QLYQS_38
Represents->
Figure QLYQS_27
The process noise covariance of the moment @>
Figure QLYQS_32
Represents->
Figure QLYQS_36
The measured noise covariance of the time instants. />
4. A vehicle state estimation method according to claim 3, characterized in that: the method comprises the following 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, and specifically comprises the following steps:
according to
Figure QLYQS_40
Vehicle state estimation and->
Figure QLYQS_41
And performing one-step prediction on the state estimation error covariance at the moment according to the following prediction equation to obtain->
Figure QLYQS_42
Predicted vehicle state estimation value and prediction error covariance at time:
Figure QLYQS_43
（5）
Figure QLYQS_44
（6）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_45
represents->
Figure QLYQS_46
One-step vehicle state prediction at a time, based on a time instant>
Figure QLYQS_47
Represents->
Figure QLYQS_48
Vehicle state estimation of a time instant>
Figure QLYQS_49
Representing the prediction error covariance; />
Figure QLYQS_50
Represents->
Figure QLYQS_51
State estimation error covariance at time.
5. A vehicle state estimation method according to claim 4, characterized in that: the observation equation residual term is expressed as:
Figure QLYQS_52
(ii) a The equation of state prediction error term is expressed as: />
Figure QLYQS_53
；
The kernel function for the residual term of the observation equation is expressed as:
Figure QLYQS_54
（7）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_55
represents a 2 norm,. Sup.>
Figure QLYQS_56
Represents a 1 norm, <' > based on>
Figure QLYQS_57
Represents a mixing factor, <' > is selected>
Figure QLYQS_58
Indicates that the nucleus is wide and/or is selected>
Figure QLYQS_59
Represents a square root function, <' > based on the square root function>
Figure QLYQS_60
Expressing an exponential function with a natural constant e as a base;
the kernel function for the prediction error term of the state equation is expressed as:
Figure QLYQS_61
（8）
the cost function is represented as:
Figure QLYQS_62
（9）。
6. a vehicle state estimation method according to claim 5, characterized in that: 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, and are expressed as:
Figure QLYQS_63
（10）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_64
represents->
Figure QLYQS_65
Vehicle state estimation at time.
7. A vehicle state estimation method according to claim 6, characterized in that: the method for 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 comprises the following steps:
to cost function about
Figure QLYQS_66
Vehicle state estimate of a time instant->
Figure QLYQS_67
Derivative and let the derivative be 0, expressed as: />
Figure QLYQS_68
（11）
In the formula (I), the compound is shown in the specification,
Figure QLYQS_69
representing a symbolic function; />
Figure QLYQS_70
Abbreviations representing kernel functions for the observation equation residual terms; />
Figure QLYQS_71
Abbreviations representing kernel functions for the state equation prediction error terms; making an approximation in the kernel function>
Figure QLYQS_72
；
Obtaining:
Figure QLYQS_73
（12）
Figure QLYQS_74
（13）
Figure QLYQS_75
（14）
in the formula (I), the compound is shown in the specification,
Figure QLYQS_76
represents->
Figure QLYQS_77
Dimension unit matrix,. Or>
Figure QLYQS_78
Represents the filter gain, <' > is greater than>
Figure QLYQS_79
The state estimation error covariance representing the time of day,
Figure QLYQS_80
represents an intermediate variable, <' > is selected>
Figure QLYQS_81
。/>
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 true CN115859039A (en) 2023-03-28
CN115859039B 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)

Cited By (2)

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

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
US20190041863A1 (en) * 2016-03-01 2019-02-07 Clarion Co., Ltd. In-vehicle device and estimation method
CN112115419A (en) * 2020-09-14 2020-12-22 深圳大学 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
CN114880874A (en) * 2022-06-07 2022-08-09 东南大学 Adaptive robust estimation method and system for parameters of unmanned surface vehicle

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190041863A1 (en) * 2016-03-01 2019-02-07 Clarion Co., Ltd. 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
CN112115419A (en) * 2020-09-14 2020-12-22 深圳大学 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
CN114880874A (en) * 2022-06-07 2022-08-09 东南大学 Adaptive robust estimation method and system for parameters of unmanned surface vehicle

Cited By (2)

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

Also Published As

Publication number Publication date
CN115859039B (en) 2023-05-23

Similar Documents

Publication Publication Date Title
Djurovic et al. A hybrid CPF-HAF estimation of polynomial-phase signals: Detailed statistical analysis
CN108470089B (en) Complex signal time delay estimation method based on least square sample fitting
WO2004036924A2 (en) Enhancing the accuracy of a location estimate
CN110501696A (en) A kind of radar target tracking method based on Doppler measurements self-adaptive processing
CN110146901A (en) Based on radial base neural net and Unscented kalman filtering multipath estimation method
CN109840517A (en) A kind of estimation of MEMS gyro noise and filtering method
Jiang et al. NLOS error mitigation with information fusion algorithm for UWB ranging systems
Colonnese et al. Generalized method of moments estimation of location parameters: Application to blind phase acquisition
CN115859039A (en) Vehicle state estimation method
Guan et al. Nonparametric variable step-size LMAT algorithm
CN115808700B (en) Carrier tracking method for dealing with ionosphere scintillation
Locubiche-Serra et al. Doubly-adaptive autoregressive Kalman filter for GNSS carrier tracking under scintillation conditions
Bodenschatz et al. Maximum-likelihood symmetric/spl alpha/-stable parameter estimation
CN109917330A (en) A kind of angle-of- arrival estimation method there are based on sparse orthogonal matching pursuit theory when phase error
Xia et al. An adaptive time delay estimator based on ETDE algorithm with noisy measurements
Fung et al. Parameter estimation of a real single tone from short data records
CN114514525B (en) Method for estimating carrier frequency, initial phase and phase noise and related equipment
CN108919628A (en) A kind of Kalman filtering and Fir filtering control method and system to combining for atomic clock
CN114895332A (en) Weak signal tracking method based on carrier-to-noise ratio adaptive Kalman filtering
CN116068586B (en) GNSS time service receiver LQG clock controller parameter design method
Bingbing et al. A RLSN-based Current Statistical Model for Target Range Tracking
CN110149290B (en) Frequency offset rough estimation method suitable for low signal-to-noise ratio high dynamic environment
Punchalard WLS algorithm for a Complex Lattice Linear Predictor for Frequency Estimation and Tracking
CN109581284B (en) Non-line-of-sight error elimination method based on interactive multiple models
Jamel Optimal Window Length and Performance Analysis of FEDS Based Adaptive Beam Forming Approach over Rayleigh Fading Channel

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