CN115905986A - Steady Kalman filtering method based on joint strategy - Google Patents

Steady Kalman filtering method based on joint strategy Download PDF

Info

Publication number
CN115905986A
CN115905986A CN202211315320.7A CN202211315320A CN115905986A CN 115905986 A CN115905986 A CN 115905986A CN 202211315320 A CN202211315320 A CN 202211315320A CN 115905986 A CN115905986 A CN 115905986A
Authority
CN
China
Prior art keywords
moment
observation
time
target
kalman filtering
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.)
Pending
Application number
CN202211315320.7A
Other languages
Chinese (zh)
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.)
Southeast University
Network Communication and Security Zijinshan Laboratory
Original Assignee
Southeast University
Network Communication and Security Zijinshan Laboratory
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 Southeast University, Network Communication and Security Zijinshan Laboratory filed Critical Southeast University
Priority to CN202211315320.7A priority Critical patent/CN115905986A/en
Publication of CN115905986A publication Critical patent/CN115905986A/en
Pending legal-status Critical Current

Links

Images

Landscapes

  • Radar Systems Or Details Thereof (AREA)

Abstract

The invention discloses a robust Kalman filtering method based on a joint strategy, which comprises the following steps of 1, establishing a Kalman filtering state transition equation and a state observation equation; step 2, estimating the initial state of the target
Figure DDA0003908561910000011
Initializing an estimation error variance P 0,0 (ii) a Step 3, obtaining the target state estimation at the moment k-1
Figure DDA0003908561910000012
And estimate error variance P k‑1,k‑1 (ii) a Before-input L time indicating variable z k The desired sequence of (a); step by stepStep 4, utilizing the weight w of the observation sample at the moment k k And an indicator variable z k Correcting the observation likelihood; step 5, carrying out posterior estimation on the hidden variables through iterative updating by means of variational Bayes inference; step 6, according to the updating result, the target state estimation at the current moment is stored
Figure DDA0003908561910000013
Error variance P k,k And the indicating variable z of the first L-1 time and the k time k The desired sequence of (a); and 7, jumping to step 3, and estimating the state at the moment k = k + 1. The invention meets the requirement of low power consumption of a real-time positioning and tracking system and improves the precision of target state estimation.

Description

Steady Kalman filtering method based on joint strategy
Technical Field
The invention belongs to the technical field of target tracking and positioning, and particularly relates to a robust Kalman filtering method based on a joint strategy.
Background
Obtaining accurate state estimates using sensor measurements is a fundamental problem for various tasks, such as target tracking, navigation, state feedback control, monitoring and optimization, and many other applications. A Kalman Filter (KF) algorithm based on linear minimum variance estimation is a commonly used state estimation method. However, in practical applications, the occurrence of measurement anomalies is a common phenomenon, which may be caused by unexpected disturbances of environmental conditions, temporary malfunctioning of sensors, calibration errors, etc. From the viewpoint of the correlation of the abnormal value in the measurement sequence, the abnormal value can be divided into an isolated abnormal value and a continuous abnormal value. Isolated outliers correspond to random outliers, which for some reason cause outliers to appear suddenly in the measurement. Continuous outliers then appear as outliers over time in the measurement due to disturbances in environmental conditions, sensor failure, sensor wear. The presence of these anomalous measurements may degrade the performance of the standard kalman filter, thereby yielding a poor estimate of the object of interest.
For the tracking problem of observation of abnormal values, two main robust filtering strategies are currently used: one is based on a heavy-tailed noise model, e.g., modeling the observed probability density function as a student t or laplacian distribution model. Other methods estimate the actual contribution of each input observation by weighting the observation covariance matrix, e.g., by introducing a gamma distribution or Wishart distribution modeled variable into the process noise covariance matrix.
Another commonly used robust strategy is based on the detection-rejection idea, i.e. outliers are automatically identified and rejected so that they cannot contribute to the state estimation. A currently common approach involves using projection statistics to distinguish outliers. And a classical recursive method is converted into a batch mode regression form, and an iterative reweighted least square algorithm is used for estimating the state. Or detecting and rejecting outliers in real time using the indicator variables modeled by the Beta-Bernoulli multi-layer prior distribution.
The algorithm is based on a single robust strategy, has a good effect of inhibiting randomly-occurring outliers, but is easy to cause the problems that the tracking trajectory is seriously deviated from the reality and filtering divergence occurs to abnormal values lasting for a period of time.
With the development of the technology, the requirements of the target positioning and tracking technology on the estimation accuracy are higher and higher. Meanwhile, in practical application, factors such as large noise of measured information, unstable channel, limited system power consumption and the like cause the faced observation scene to be more and more complex. Therefore, it is of great significance to find a method which can not only adapt to a complex tracking and positioning scene, but also ensure high-precision positioning.
Disclosure of Invention
The invention aims to provide a steady Kalman filtering method based on a combined strategy to solve the technical problems of adapting to complex tracking and positioning scenes and ensuring high-precision positioning.
In order to solve the technical problems, the specific technical scheme of the invention is as follows:
a robust Kalman filtering method based on a joint strategy comprises the following steps:
step 1, establishing a motion equation of Kalman filtering;
step 2, establishing an observation equation of Kalman filtering;
step 3, obtaining the state of the target at the moment k-1
Figure BDA0003908561890000021
And estimate error variance P k-1,k-1 (ii) a Measurement information y at time k k (ii) a And a desired sequence of preceding L moment indicating variables<z k-L >,<z k-L+1 >,…<z k-1 >}；
Step 4, adopting a steady Kalman filtering method based on a joint strategy, and introducing a weight w at the moment k k And an indicator variable z k Correcting the observation likelihood function;
and 5, by means of variational Bayes inference, iteratively updating the target state at the k moment
Figure BDA0003908561890000022
Estimate error variance P k,k And carrying out posterior estimation on the hidden variable;
step 6, outputting the state of the target at the time k
Figure BDA0003908561890000023
And estimate error variance P k,k (ii) a And an expected sequence of indicating variables at the first L-1 moment and the first k moment<z k-L+1 >,<z k-L+2 >,…<z k >}; when a new observed value is input, returning to the step 3 for estimating the next moment; and if no new observation input exists, finishing the filtering.
Further, the step 1 of establishing the motion equation of the kalman filter specifically includes:
x k ＝Ax k-1 +u k
in the formula
Figure BDA0003908561890000024
[·] T Represents a transposition of the vector; x is the number of k ，y k Is the target position at the time point k,
Figure BDA0003908561890000025
is the target speed at time k->
Figure BDA0003908561890000026
Is the target acceleration at time k; />
Figure BDA0003908561890000027
Is process noise, where Σ is the process noise covariance matrix; a is a state transitionMatrix:
Figure BDA0003908561890000028
wherein, Δ T is the observation time interval for obtaining the target state, I 2 Representing a second order unit matrix.
Further, the establishing of the observation equation of the kalman filter in step 2 includes:
y k ＝Cx k +v k
in the formula y k Is measurement information at time k, x k The target state at the moment k;
Figure BDA0003908561890000031
is the observation noise, where R is the observation noise covariance matrix; c is an observation matrix:
C＝[I 2 0 2 0 2 ]。
further, step 3 comprises the following steps:
step 3.1, initialization at the moment when k = 0: given target state in combination with a priori knowledge
Figure BDA0003908561890000032
Estimate error variance P 0,0 C, super parameter c 0 、d 0 、e 0 、f 0 And hidden markov chain order L; determining a process noise covariance matrix sigma and an observation noise covariance matrix R according to the condition of target motion and the condition of measurement information;
step 3.2, initializing i =0 when k is larger than or equal to 1;<z k >＝1；<w k >＝1；
step 3.3, when k is more than or equal to 1 and less than the moment L +1 and i =0, inputting the expected sequence of the indicating variable at the previous k-1 moment<z 1 >,<z 2 >,…<z k-1 >}；
When k ≧ L +1 time, i =0, a desired sequence of the preceding L time indication variable is input<z k-L >,<z k-L+1 >,…<z k-1 >}。
Further, step 4 specifically includes the following steps:
step 4.1, standard Gaussian distribution observation likelihood probability density function:
Figure BDA0003908561890000033
wherein C represents an observation matrix, and R represents an observation noise covariance matrix;
step 4.2, introduce the weight w of the moment k k Modifying the observation likelihood function:
Figure BDA0003908561890000034
in the formula w k Obeying a Gamma distribution:
w k ～Gamma(w k |c 0 ,d 0 )
wherein, c 0 And d 0 Is a hyper-parameter of Gamma distribution;
step 4.3, introducing an indicator variable z of the k time on the basis of the step 4.2 k The observation likelihood function is further modified:
Figure BDA0003908561890000041
in the formula z k Markov chain belonging to L-order discrete sequence
Figure BDA0003908561890000042
Wherein
Figure BDA0003908561890000043
Modeling by Beta-Bernoulli multilayer prior distribution:
Figure BDA0003908561890000044
π j ～Beta(π j |e 0 ,f 0 ),
wherein the content of the first and second substances,
Figure BDA0003908561890000045
is an indicator variable modeled by a hidden Markov chain at the moment k, and follows Bernoulli distribution; pi j Modeling by Beta distribution for the desired variable of the Bernoulli distribution; e.g. of a cylinder 0 And f 0 Is the hyper-parameter of the Beta distribution.
Further, step 5 specifically includes the following steps:
step 5.1, kalman Filter prediction
Figure BDA0003908561890000046
The formula of (1) is:
Figure BDA0003908561890000047
wherein A is a state transition matrix;
step 5.2, kalman Filter prediction P k,k-1 The formula of (1) is:
P k,k-1 ＝AP k-1,k-1 A T +Σ；
wherein Σ is a process noise covariance matrix;
step 5.3, entering the ith cycle iteration, deducing according to variational Bayes, and modifying the Kalman gain K k Expressed as:
Figure BDA0003908561890000048
expectation of using weight<w k >And indicating the expectation of the variable<z k >Scaling the noise covariance matrix;
step 5.4, when<z k >＞10 -3 Time, target state
Figure BDA0003908561890000049
And an estimated error variance>
Figure BDA00039085618900000410
The posterior estimation formula of (a) is:
Figure BDA00039085618900000411
Figure BDA0003908561890000051
when in use<z k >≤10 -3 Time, target state
Figure BDA0003908561890000052
And the estimated error variance->
Figure BDA0003908561890000053
The posterior estimation formula of (a) is:
Figure BDA0003908561890000054
/>
Figure BDA0003908561890000055
step 5.5, implicit variables in step 5.3
Figure BDA0003908561890000056
Following the Bernoulli distribution, the posterior expectation is updated according to variational bayes inference as:
Figure BDA0003908561890000057
wherein the content of the first and second substances,
Figure BDA0003908561890000058
and &>
Figure BDA0003908561890000059
Expressed as:
Figure BDA00039085618900000510
Figure BDA00039085618900000511
wherein the content of the first and second substances,
η k ＝<lnw k >-M ln(2π)-ln|R|-<w k >ρ k ，
ρ k expressed as:
Figure BDA00039085618900000512
wherein Tr (·) is the trace of the matrix;
step 5.6, implicit variables in step 5.5
Figure BDA00039085618900000513
Obeying to Beta distribution, according to variational Bayes inference, the posterior expectation is updated as:
Figure BDA00039085618900000514
Figure BDA00039085618900000515
wherein the content of the first and second substances,
Figure BDA00039085618900000516
Figure BDA0003908561890000061
step 5.7, implicit variable w in step 5.3 k Following Gamma distribution, according to variational Bayes inference, the posterior expectation is updated as:
Figure BDA0003908561890000062
Figure BDA0003908561890000063
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003908561890000064
Figure BDA0003908561890000065
step 5.8, calculating the value of the relative increment tau of the iterative estimation state:
Figure BDA0003908561890000066
/>
step 5.9, the iteration times are increased: i = i +1;
step 5.10, judging if tau is more than 10 -6 If yes, the loop iteration is exited, and the step 5.11 is executed; if τ is less than or equal to 10 -6 Then returning to the step 5.3 to continue the loop iteration;
step 5.11, outputting the state of the target at the k moment
Figure BDA0003908561890000067
And the estimated error variance->
Figure BDA0003908561890000068
And an expected sequence of indicating variables at the first L-1 moment and the first k moment<z k-L+1 >,<z k-L+2 >,…<z k >}。
The robust Kalman filtering method based on the joint strategy has the following advantages:
1. the invention introduces the weight w of the moment k k And an indicator variable z k Modifying the observation likelihood function, w k Is a weight variable, z, modeled by a Gamma distribution k The method is an indicator variable modeled by Beta-Bernoulli distribution, so that an abnormal value identification and processing strategy based on a heavy tail noise model and a detection-rejection strategy is fused, and the method has better discovery and inhibition capability on randomly-occurring isolated abnormal values, thereby improving the robustness of Kalman filtering.
2. Indicator variable z of the invention k The Markov chain is used for modeling, and the method has a good inhibition effect on abnormal values with time correlation.
3. The tracking filtering framework of the invention is based on Kalman filtering, utilizes variational Bayes inference to make posterior estimation, has simple overall realization and small calculated amount, and meets the requirement of low power consumption of a positioning and tracking system.
Drawings
FIG. 1 is a flow chart of the robust Kalman filtering based on the joint strategy of the present invention;
FIG. 2 is a graph comparing the moving position component of the object and the data collected by the sensor in example 1;
FIG. 2 (a) is a diagram showing the lateral position of the movement of an object in embodiment 1 of the present invention;
FIG. 2 (b) is a graph showing the actual lateral position measurement values in embodiment 1 of the present invention;
FIG. 2 (c) is a view showing the longitudinal position of the movement of the object in embodiment 1 of the present invention;
FIG. 2 (d) is a graph showing the actual longitudinal position measurement values in embodiment 1 of the present invention;
FIG. 3 is a comparison graph of the filtered estimated trajectory and the positioning error curve of different algorithms in embodiment 1 of the present invention;
FIG. 3 (a) is a comparison graph of the estimation results of the target position states in embodiment 1 of the present invention;
FIG. 3 (b) is a comparison graph of the position estimation Root Mean Square Error (RMSE) of the algorithm in example 1 of the present invention;
FIG. 4 is a comparison graph of the variation curves of the positioning errors of different algorithms along with the observation abnormal proportion in embodiment 1 of the present invention;
FIG. 5 is a graph showing a comparison between a moving position component of an object and data collected by a sensor in embodiment 2 of the present invention;
FIG. 5 (a) is a diagram showing the lateral position of the movement of an object in embodiment 2 of the present invention;
FIG. 5 (b) is a graph showing the actual lateral position measurement values in embodiment 2 of the present invention;
FIG. 5 (c) is a view showing the longitudinal position of the movement of an object in embodiment 2 of the present invention;
FIG. 5 (d) is a graph showing the measured values of the actual longitudinal position in embodiment 2 of the present invention;
FIG. 6 is a comparison graph of the filtering estimation trajectory and the positioning error curve of different algorithms in embodiment 2 of the present invention;
FIG. 6 (a) is a comparison graph of the target position state estimation results in embodiment 2 of the present invention;
FIG. 6 (b) is a comparison graph of the position estimation Root Mean Square Error (RMSE) of the algorithm in example 2 of the present invention;
FIG. 7 is a comparison graph of the variation curves of the positioning errors of different algorithms along with the observation abnormal proportion in embodiment 2 of the present invention.
Detailed Description
In order to better understand the purpose, structure and function of the present invention, a robust kalman filtering method based on a joint strategy according to the present invention is described in detail below with reference to the accompanying drawings.
The first embodiment is as follows:
the invention discloses a steady Kalman filtering method based on a combined strategy, which combines a double tail noise model and a detection-rejection strategy, has a good inhibition effect on isolated abnormal values and continuous abnormal values in measurement, and realizes real-time and steady filtering and tracking of a sensor in a complex scene. In order to verify the effectiveness of the method, the two-dimensional trajectory tracking is taken as an example in the embodiment, a situation that an isolated abnormal value exists in data acquired by a simulation sensor is firstly simulated, and a formula for generating a pollution observation value is expressed as follows:
y k ＝Cx k +v k +r k
wherein x is k A state vector that is a target; y is k Is a measurement vector; c is a position observation matrix; v. of k In the form of additive gaussian noise, the noise,
Figure BDA0003908561890000081
wherein R is an observation noise covariance matrix; r is a radical of hydrogen k Is to generate a non-gaussian noise term in the observation, expressed as:
Figure BDA0003908561890000082
c k ～Bernoulli(p k )，
wherein U (. Cndot.) represents a uniform distribution, [ a ] m ,b m ]A domain representing a uniform distribution of the mth dimension outlier; p is a radical of formula k Indicating the probability of occurrence of isolated outliers. In this example, [ a ] 1 ,b 1 ]＝[-10,10]，[a 2 ,b 2 ]＝[-20,20]，p k ＝0.2。
The specific steps of the robust kalman filtering method based on the joint strategy disclosed in this embodiment are shown in fig. 1, and include:
step 1, establishing a motion equation of Kalman filtering:
x k ＝Ax k-1 +u k
in the formula
Figure BDA0003908561890000083
x k ，y k Target position for time k>
Figure BDA0003908561890000084
Target speed for time k>
Figure BDA0003908561890000085
Is the target acceleration at time k; />
Figure BDA0003908561890000086
Is process noise, where Σ is the process noise covarianceA difference matrix; a is a state transition matrix:
Figure BDA0003908561890000087
wherein Δ T =0.01s is an observation time interval for acquiring a target state; i is 2 Representing a second order unit matrix.
Step 2, establishing an observation equation of Kalman filtering:
y k ＝Cx k +v k
in the formula y k Is measurement information at time k, x k The target state at the moment k;
Figure BDA0003908561890000091
for observation noise, R is an observation noise covariance matrix; c is an observation matrix:
C＝[I 2 0 2 0 2 ]
step 3, obtaining the state of the target at the moment k-1
Figure BDA0003908561890000092
And estimate error variance P k-1,k-1 (ii) a Measurement information y at time k k (ii) a And a desired sequence of preceding L moment indicating variables<z k-L >,<z k-L+1 >,…<z k-1 >}；
Before step 3, the method also comprises the following steps:
step 3.1, initialization at the moment when k = 0: given target state in combination with a priori knowledge
Figure BDA0003908561890000093
Estimation error covariance P 0,0 In this embodiment->
Figure BDA0003908561890000094
P 0,0 ＝10 -1 I 6 (ii) a Given hyperparameter c 0 、d 0 、e 0 、f 0 And hidden markov chain order L; the situation of moving by the target and the situation of measuring informationThe condition determination process noise covariance matrix Σ and the observation noise covariance matrix R, where Σ =10 in this embodiment -3 I 6 ，R＝10 -1 I 2 ；
Step 3.2, initializing i =0 when k is larger than or equal to 1;<z k >＝1；<w k >＝1；
step 3.3, when k is more than or equal to 1 and less than the moment L +1 and i =0, inputting the expected sequence of the indicating variable at the previous k-1 moment<z 1 >,<z 2 >,…<z k-1 >}；
When k ≧ L +1 time, i =0, a desired sequence of the preceding L time indication variable is input<z k-L >,<z k-L+1 >,…<z k-1 >}；
Step 4, adopting a steady Kalman filtering method based on a joint strategy, and introducing a weight w at the moment k k And an indicator variable z k Correcting the observation likelihood function;
step 4.1, standard Gaussian distribution observation likelihood probability density function:
Figure BDA0003908561890000095
wherein C represents an observation matrix, and R represents an observation noise covariance matrix;
step 4.2, introduce the weight w of the moment k k Modifying the observation likelihood function:
Figure BDA0003908561890000096
in the formula w k Obeying a Gamma distribution:
w k ～Gamma(w k |c 0 ,d 0 )
wherein, c 0 And d 0 Is a hyper-parameter of the Gamma distribution;
step 4.3, introducing an indicator variable z of the k time point on the basis of the step 4.2 k The observation likelihood function is further modified:
Figure BDA0003908561890000101
in the formula z k Markov chain belonging to L-order discrete sequence
Figure BDA0003908561890000102
Wherein
Figure BDA0003908561890000103
Modeling by Beta-Bernoulli multilayer prior distribution:
Figure BDA0003908561890000104
π j ～Beta(π j |e 0 ,f 0 ),
wherein the content of the first and second substances,
Figure BDA0003908561890000105
is an indicator variable modeled by a hidden Markov chain at the moment k, and follows Bernoulli distribution; pi j Modeling by a Beta distribution for the desired variable of the Bernoulli distribution; e.g. of the type 0 And f 0 Is a hyper-parameter of the Beta distribution;
step 5, by means of variational Bayes inference, the target state of the k moment is updated through iteration
Figure BDA0003908561890000106
Estimate error variance P k,k And carrying out posterior estimation on the hidden variable;
step 5.1, kalman Filter prediction
Figure BDA0003908561890000107
The formula of (1) is:
Figure BDA0003908561890000108
wherein A is a state transition matrix;
step 5.2, kalman Filter prediction P k,k-1 The formula of (1) is:
P k,k-1 ＝AP k-1,k-1 A T +Σ；
wherein Σ is a process noise covariance matrix;
step 5.3, entering the ith cycle iteration, deducing according to variational Bayes, and modifying the Kalman gain K k Expressed as:
Figure BDA0003908561890000109
expectation of using weight<w k >And indicating the expectation of the variable<z k >Scaling the noise covariance matrix;
step 5.4, when<z k >＞10 -3 Time, target state
Figure BDA0003908561890000111
And an estimated error variance>
Figure BDA0003908561890000112
The posterior estimation formula of (a) is:
Figure BDA0003908561890000113
Figure BDA0003908561890000114
when in use<z k >≤10 -3 Time, target state
Figure BDA0003908561890000115
And the estimated error variance->
Figure BDA0003908561890000116
The posterior estimation formula of (a) is:
Figure BDA0003908561890000117
Figure BDA0003908561890000118
step 5.5, implicit variables in step 5.3
Figure BDA0003908561890000119
Following the Bernoulli distribution, the posterior expectation is updated according to variational bayes inference as:
Figure BDA00039085618900001110
wherein the content of the first and second substances,
Figure BDA00039085618900001111
and &>
Figure BDA00039085618900001112
Expressed as:
Figure BDA00039085618900001113
Figure BDA00039085618900001114
wherein the content of the first and second substances,
η k ＝<ln w k >-M ln(2π)-ln|R|-<w k >ρ k ，
ρ k expressed as:
Figure BDA00039085618900001115
wherein Tr (·) is the trace of the matrix;
step 5.6, hiding in step 5.5Variables of
Figure BDA00039085618900001116
Obeying to Beta distribution, according to variational Bayes inference, the posterior expectation is updated as:
Figure BDA00039085618900001117
Figure BDA0003908561890000121
wherein the content of the first and second substances,
Figure BDA0003908561890000122
/>
Figure BDA0003908561890000123
step 5.7, implicit variable w in step 5.3 k Following Gamma distribution, according to variational Bayes inference, the posterior expectation is updated as:
Figure BDA0003908561890000124
Figure BDA0003908561890000125
wherein the content of the first and second substances,
Figure BDA0003908561890000126
Figure BDA0003908561890000127
step 5.8, calculating the value of the relative increment tau of the iterative estimation state:
Figure BDA0003908561890000128
step 5.9, the iteration times are increased: i = i +1;
step 5.10, judging if tau is more than 10 -6 If yes, the loop iteration is exited, and the step 6 is executed; if tau is less than or equal to 10 -6 If yes, returning to the step 5.3 to continue the loop iteration;
step 6, outputting the state of the target at the time k
Figure BDA0003908561890000129
And estimate error variance P k,k (ii) a And an expected sequence of indicating variables at the first L-1 moment and the first k moment<z k-L+1 >,<z k-L+2 >,…<z k >}; when a new observed value is input, returning to the step 3 for estimating the next moment; and if no new observation input exists, ending the filtering.
As shown in fig. 2, fig. 2- (a) and fig. 2- (c) are real motion trajectories of the target in the horizontal and vertical position components, respectively, and fig. 2- (b) and fig. 2- (d) are observed data obtained by sampling, respectively. It can be seen that a large number of randomly occurring isolated outliers, i.e., outliers significantly deviating from observations containing gaussian noise, are distributed in the horizontal and vertical position dimensions of the observation data of the input sensor.
Further, in this embodiment, the tracking effect at each time is evaluated by using Root Mean Square Error (RMSE) of positioning:
Figure BDA0003908561890000131
wherein N is 0 Denotes the number of Monte Carlo experiments, N in this example 0 ＝500；
Figure BDA0003908561890000132
Represents the horizontal and vertical coordinates estimated by the k time algorithm in the nth Monte Carlo experiment, and then gets the result of the evaluation>
Figure BDA0003908561890000133
Representing the actual horizontal and vertical coordinates of the target at the k moment in the nth Monte Carlo experiment;
further, a Time-averaged Root Mean Square Error (TRMSE) is defined:
Figure BDA0003908561890000134
where K represents the total number of observation sample points.
Further, under the interference of different types of observation abnormal values, a simulation comparison experiment is carried out with the existing robust filter Outlier-robust Kalman filtering (OR-KF) and Outlier Detection Kalman filtering (OD-KF), and the superiority of continuous-measures-robust Kalman filtering (CMR-KF) is fully verified. Wherein, the OR-KF method only introduces the weight w k Modifying the observation likelihood function; the OD-KF method introduces only the indicator variable z k The observation likelihood function is modified.
FIG. 3- (a) shows the results of the OR-KF, OD-KF and CMR-KF methods trajectory estimation compared to the true trajectory. FIG. 3- (b) shows the RMSE curves of the three methods at each sampling point. Obviously, the CMR-KF method disclosed by the invention is almost the lowest in positioning error of each sampling point in the general view, has the characteristic of good robustness aiming at randomly occurring single-point abnormal values, and is higher in state estimation precision.
Further, an abnormal control factor, i.e. the occurrence probability p of an isolated abnormal value, is measured k Changing from 0.05 to 0.5, keeping other conditions unchanged, carrying out Monte Carlo simulation experiments for 500 times on different value ratios, and counting the time-averaged root mean square error (TRMSE) of each tracking method, as shown in FIG. 4. It can be seen that the TRMSE for CMR-KF is always the lowest, although the TRMSE for each method increases as the proportion of outliers increases.
The method uses a combined heavy tail noise model and a detection-rejection strategy for standard Kalman filtering, and automatically identifies and processes abnormal values for input observation samples through a weight variable modeled by Gamma distribution and an indicator variable modeled by Beta-Bernoulli distribution. The method is different from the traditional Kalman filtering method, and does not distinguish the received observation samples; compared with a steady filter with a single strategy, the method provided by the invention adopts a combined abnormal value processing strategy, and has a better inhibition effect on the abnormal value. The embodiment verifies that the method can effectively realize the robust filtering when the isolated abnormal value exists in the observation sample.
Example two:
in order to verify the filtering effect of the method disclosed by the invention in a more complex observation scene, the embodiment changes the form of an input observation sample on the basis of the first embodiment, so that the sensor observation has not only isolated abnormal values which occur randomly but also continuous abnormal values with time correlation, and observes the estimation result of the robust kalman filtering method based on the joint strategy. In the embodiment, two-dimensional trajectory tracking is taken as an example, and under the condition that an isolated abnormal value and a continuous abnormal value exist in data acquired by a simulation sensor at the same time, a non-Gaussian noise item r in a pollution observation value is generated k Is expressed as:
Figure BDA0003908561890000141
in the formula, r k Two sections of continuous abnormal values are contained at the time when k is more than or equal to 200 and less than 250 and the time when k is more than or equal to 600 and less than 650, and the isolated abnormal values existing at other times are expressed as follows:
Figure BDA0003908561890000142
c k ～Bernoulli(p k )，
wherein U (. Cndot.) represents a uniform distribution, [ a ] m ,b m ]A domain representing a uniform distribution of m-th dimension outliers. In this example, [ a ] 1 ,b 1 ]＝[-30,30]，[a 2 ,b 2 ]＝[-20,20]，p k ＝0.2。Other filter initialization conditions: target initial state estimation
Figure BDA0003908561890000143
Initial estimation error variance P 0,0 ＝diag[0.1,0.1,0.1,0.1,0.01,0.01]。
As shown in fig. 5, fig. 5- (a) and 5- (c) are real motion trajectories of the target in the horizontal and vertical position components, respectively, and fig. 5- (b) and 5- (d) are observed data obtained by sampling, respectively. It can be seen that the observation data of the input sensor has two sections of abnormal observation lasting for a period of time in the horizontal and longitudinal position dimensions in addition to the isolated abnormal values. FIG. 6- (a) shows the results of the OR-KF, OD-KF and CMR-KF method trajectory estimation compared to the true trajectory. FIG. 6- (b) shows the RMSE curves of the three methods at each sampling point. The OR-KF method is due to the introduction of only w k The variables weight the observation covariance matrix R, and the outliers are still received with a certain probability at the moment they occur, so the accumulated error increases significantly. Obviously, the CMR-KF method disclosed by the invention is almost the lowest in positioning error of each sampling point in the general view, has the characteristic of good robustness for isolated abnormal values and continuous abnormal values with time correlation, and is higher in state estimation precision.
Further, given an isolated outlier probability p k And a width β of each segment of consecutive outliers, the mixed outlier ratio λ is expressed as:
Figure BDA0003908561890000151
further, the isolated outlier probability p is fixed k =0.2, the proportion of the mixed outlier is changed by changing β so that λ changes from 0.2 to 0.5. At this time, the non-Gaussian noise term r k Expressed as:
Figure BDA0003908561890000152
in the formula (I), the compound is shown in the specification,
Figure BDA0003908561890000153
c k ～Bernoulli(p k ). Other conditions are unchanged, monte carlo simulation experiments are performed for 500 times on different value ratios, and the time-averaged root mean square error (TRMSE) of each tracking method is counted, as shown in fig. 7. It can be seen that the error of the OR-KF method increases significantly for λ > 0.4, whereas the error of the CMR-KF method disclosed in the present invention is always the lowest TRMSE, although it increases with increasing proportion of mixed outliers. The embodiment verifies that the method can effectively realize the stable filtering when the isolated abnormal value and the continuous abnormal value exist in the observation sample at the same time.
It is to be understood that the present invention has been described with reference to certain embodiments and that various changes in form and details may be made therein by those skilled in the art without departing from the spirit and scope of the invention. In addition, many modifications may be made to adapt a particular situation or material to the teachings of the invention without departing from the essential scope thereof. Therefore, it is intended that the invention not be limited to the particular embodiment disclosed, but that the invention will include all embodiments falling within the scope of the appended claims.

Claims (6)

1. A robust Kalman filtering method based on a joint strategy is characterized by comprising the following steps:
step 1, establishing a motion equation of Kalman filtering;
step 2, establishing an observation equation of Kalman filtering;
step 3, obtaining the state of the target at the moment k-1
Figure FDA0003908561880000011
And estimate the error variance P k-1,k-1 (ii) a Measurement information y at time k k (ii) a And a desired sequence of preceding L moment indicator variables<z k-L >,<z k-L+1 >,…<z k-1 >}；
Step 4, adopting a steady Kalman filtering method based on a joint strategy, and introducing a weight w of the moment k k And an indicating variable z k Correcting the observation likelihood function;
step 5, by means of variational Bayes inference, the target state of the k moment is updated through iteration
Figure FDA0003908561880000012
Estimate error variance P k,k And carrying out posterior estimation on the hidden variable;
step 6, outputting the state of the target at the time k
Figure FDA0003908561880000013
And estimate error variance P k,k (ii) a And an expected sequence of indicating variables at the first L-1 moment and the first k moment<z k-L+1 >,<z k-L+2 >,…<z k >}; when a new observed value is input, returning to the step 3 for estimating the next moment; and if no new observation input exists, finishing the filtering.
2. The robust kalman filtering method based on the joint strategy according to claim 1, wherein the step 1 of establishing the equation of motion of the kalman filtering specifically comprises:
x k ＝Ax k-1 +u k
in the formula
Figure FDA0003908561880000014
[·] T Represents a transposition of the vector; x is a radical of a fluorine atom k ，y k Target position for time k>
Figure FDA0003908561880000015
Figure FDA0003908561880000016
Is the target speed at time k->
Figure FDA0003908561880000017
Is the target acceleration at time k; />
Figure FDA0003908561880000018
Is process noise, where Σ is the process noise covariance matrix; a is a state transition matrix:
Figure FDA0003908561880000019
wherein Δ T is the observation time interval for obtaining the target state, I 2 Representing a second order unit matrix.
3. The robust kalman filtering method based on joint strategy according to claim 1, wherein the step 2 of establishing the observation equation of kalman filtering comprises:
y k ＝Cx k +v k
in the formula y k Is measurement information at time k, x k The target state at the moment k;
Figure FDA0003908561880000021
is the observed noise, where R is the observed noise covariance matrix; c is an observation matrix:
C＝[I 2 0 2 0 2 ]。
4. the joint strategy based robust kalman filtering method according to claim 1, wherein the step 3 comprises the following steps:
step 3.1, initialization at the moment when k = 0: given target state in combination with a priori knowledge
Figure FDA0003908561880000024
Estimate error variance P 0,0 C, super parameter c 0 、d 0 、e 0 、f 0 And hidden markov chain order L; from the situation of the object movement and the situation of the measurement informationDetermining a process noise covariance matrix sigma and an observation noise covariance matrix R;
step 3.2, initializing i =0 when k is larger than or equal to 1;<z k >＝1；<w k >＝1；
step 3.3, when k is more than or equal to 1 and less than the moment L +1 and i =0, inputting the expected sequence of the indicating variable at the previous k-1 moment<z 1 >,<z 2 >,…<z k-1 >}；
When k ≧ L +1 time, i =0, a desired sequence of the preceding L time indication variable is input<z k-L >,<z k-L+1 >,…<z k-1 >}。
5. The robust kalman filtering method based on the joint strategy according to claim 1, wherein the step 4 specifically includes the steps of:
step 4.1, standard Gaussian distribution observation likelihood probability density function:
Figure FDA0003908561880000022
wherein C represents an observation matrix, and R represents an observation noise covariance matrix;
step 4.2, introduce the weight w of the moment k k Modifying the observation likelihood function:
Figure FDA0003908561880000023
in the formula w k Obeying a Gamma distribution:
w k ～Gamma(w k |c 0 ,d 0 )
wherein, c 0 And d 0 Is a hyper-parameter of the Gamma distribution;
step 4.3, introducing an indicator variable z of the k time point on the basis of the step 4.2 k The observation likelihood function is further modified:
Figure FDA0003908561880000031
in the formula z k Markov chain belonging to discrete sequence of order L
Figure FDA0003908561880000032
Wherein->
Figure FDA0003908561880000033
Figure FDA0003908561880000034
Modeling by Beta-Bernoulli multilayer prior distribution:
Figure FDA0003908561880000035
π j ～Beta(π j |e 0 ,f 0 ),
wherein the content of the first and second substances,
Figure FDA0003908561880000036
is an indicator variable modeled by a hidden Markov chain at the moment k, and follows Bernoulli distribution; pi j Modeling by Beta distribution for the desired variable of the Bernoulli distribution; e.g. of the type 0 And f 0 Is the hyper-parameter of the Beta distribution.
6. The robust kalman filtering method based on the joint strategy according to claim 1, wherein the step 5 specifically includes the steps of:
step 5.1, kalman Filter prediction
Figure FDA0003908561880000037
The formula of (1) is:
Figure FDA0003908561880000038
wherein A is a state transition matrix;
step 5.2, kalman Filter prediction P k,k-1 The formula of (1) is as follows:
P k,k-1 ＝AP k-1,k-1 A T +Σ；
wherein Σ is a process noise covariance matrix;
step 5.3, entering the ith cycle iteration, deducing according to variational Bayes, and modifying the Kalman gain K k Expressed as:
Figure FDA0003908561880000039
expectation of using weight<w k >And indicating the expectation of the variable<z k >Scaling the noise covariance matrix;
step 5.4, when<z k >＞10 -3 Time, target state
Figure FDA00039085618800000310
And the estimated error variance->
Figure FDA00039085618800000311
The posterior estimation formula of (a) is: />
Figure FDA0003908561880000041
Figure FDA0003908561880000042
When the temperature is higher than the set temperature<z k >≤10 -3 Time, target state
Figure FDA0003908561880000043
And the estimated error variance->
Figure FDA0003908561880000044
The posterior estimation formula of (a) is:
Figure FDA0003908561880000045
Figure FDA0003908561880000046
step 5.5, implicit variables in step 5.3
Figure FDA0003908561880000047
Following the Bernoulli distribution, the posterior expectation is updated according to variational bayes inference as:
Figure FDA0003908561880000048
wherein the content of the first and second substances,
Figure FDA0003908561880000049
and &>
Figure FDA00039085618800000410
Expressed as:
Figure FDA00039085618800000411
Figure FDA00039085618800000412
wherein the content of the first and second substances,
η k ＝<lnw k >-Mln(2π)-ln|R|-<w k >ρ k ，
ρ k expressed as:
Figure FDA00039085618800000413
wherein Tr (·) is the trace of the matrix;
step 5.6, implicit variables in step 5.5
Figure FDA00039085618800000414
Obeying to Beta distribution, according to variational Bayes inference, the posterior expectation is updated as:
Figure FDA00039085618800000415
Figure FDA00039085618800000416
wherein the content of the first and second substances,
Figure FDA0003908561880000051
Figure FDA0003908561880000052
step 5.7, implicit variable w in step 5.3 k Following the Gamma distribution, the posterior expectation is updated as follows according to the variational Bayes inference:
Figure FDA0003908561880000053
Figure FDA0003908561880000054
wherein the content of the first and second substances,
Figure FDA0003908561880000055
/>
Figure FDA0003908561880000056
step 5.8, calculating the value of the relative increment tau of the iterative estimation state:
Figure FDA0003908561880000057
step 5.9, the iteration times are increased: i = i +1;
step 5.10, judging if tau is more than 10 -6 If yes, the loop iteration is exited, and the step 5.11 is executed; if tau is less than or equal to 10 -6 If yes, returning to the step 5.3 to continue the loop iteration;
step 5.11, outputting the state of the target at the k moment
Figure FDA0003908561880000058
And an estimated error variance>
Figure FDA0003908561880000059
And an expected sequence of indicating variables at the first L-1 moment and the first k moment<z k-L+1 >,<z k-L+2 >,…<z k >}。/>
CN202211315320.7A 2022-10-26 2022-10-26 Steady Kalman filtering method based on joint strategy Pending CN115905986A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211315320.7A CN115905986A (en) 2022-10-26 2022-10-26 Steady Kalman filtering method based on joint strategy

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211315320.7A CN115905986A (en) 2022-10-26 2022-10-26 Steady Kalman filtering method based on joint strategy

Publications (1)

Publication Number Publication Date
CN115905986A true CN115905986A (en) 2023-04-04

Family

ID=86478512

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211315320.7A Pending CN115905986A (en) 2022-10-26 2022-10-26 Steady Kalman filtering method based on joint strategy

Country Status (1)

Country Link
CN (1) CN115905986A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117093824A (en) * 2023-10-20 2023-11-21 北京开运联合信息技术集团股份有限公司 Space target behavior monitoring method

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117093824A (en) * 2023-10-20 2023-11-21 北京开运联合信息技术集团股份有限公司 Space target behavior monitoring method
CN117093824B (en) * 2023-10-20 2024-01-19 北京开运联合信息技术集团股份有限公司 Space target behavior monitoring method

Similar Documents

Publication Publication Date Title
CN110503071B (en) Multi-target tracking method based on variational Bayesian label multi-Bernoulli superposition model
CN111178385B (en) Target tracking method for robust online multi-sensor fusion
CN109829938B (en) Adaptive fault-tolerant volume Kalman filtering method applied to target tracking
CN111985093B (en) Adaptive unscented Kalman filtering state estimation method with noise estimator
Konatowski et al. Comparison of estimation accuracy of EKF, UKF and PF filters
CN107045125B (en) Interactive multi-model radar target tracking method based on predicted value measurement conversion
Hao et al. Comparison of unscented kalman filters
CN111127523B (en) Multi-sensor GMPHD self-adaptive fusion method based on measurement iteration update
CN111711432B (en) Target tracking algorithm based on UKF and PF hybrid filtering
CN115905986A (en) Steady Kalman filtering method based on joint strategy
CN116047498A (en) Maneuvering target tracking method based on maximum correlation entropy extended Kalman filtering
CN114626307B (en) Distributed consistent target state estimation method based on variational Bayes
CN109115228B (en) Target positioning method based on weighted least square volume Kalman filtering
CN111798494A (en) Maneuvering target robust tracking method under generalized correlation entropy criterion
CN112986977B (en) Method for overcoming radar extended Kalman track filtering divergence
CN110532517A (en) Gas pipeline method for parameter estimation based on improved ARUKF
CN112034445B (en) Vehicle motion trail tracking method and system based on millimeter wave radar
CN111340853B (en) Multi-sensor GMPHD self-adaptive fusion method based on OSPA iteration
CN106871905B (en) Gaussian filtering substitution framework combined navigation method under non-ideal condition
CN111262556A (en) Multi-target tracking method for simultaneously estimating unknown Gaussian measurement noise statistics
CN115828533A (en) Interactive multi-model robust filtering method based on Student&#39;s t distribution
CN113391285B (en) Target tracking smoothing method for measuring flicker noise under random delay
CN115859626A (en) Self-adaptive unscented Kalman filter design method for periodic moving target
CN115685128A (en) Radar target tracking algorithm and electronic equipment under maneuvering target scene
Kellermann et al. Estimation of unknown system states based on an adaptive neural network and Kalman 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