Maneuvering target tracking -- current statistical model (CS model) extended Kalman filter / unscented Kalman filter matlab implementation

Maneuvering target tracking —— Current statistical model （CS Model ） Extended Kalman filter / Unscented Kalman filter matlab Realization

Originality is not easy. , Please give a compliment to all the big guys passing by

Discussion on maneuvering target tracking 、 Technical support welcome to contact , You can also send private messages on the website
WX: ZB823618313

chart ： Adjust , Learning is so tiring

1. Understanding of maneuvering target tracking

1.1. Understanding of maneuvering target tracking

   Maneuvering target tracking has always been a difficult and key problem in the field of target tracking . The establishment of target motion model and filtering algorithm are two important factors in target tracking . Because the maneuvering of the target is unpredictable , It makes it difficult for us to establish an accurate target motion model . How to establish an effective model to reflect the real trajectory of the target is an urgent problem to be solved in the high maneuvering target tracking system . After nearly 30 years of research , Many important achievements have been made in this field .

Personal understanding ： Maneuvering target tracking has three elements ：

Modeling of tracked target （ This blog focuses on ： Current statistical model ）
Sensors measure （ Another blog introduces ）
Filter design （ See target tracking column ）

   From the algorithmic level , In the target tracking system , The commonly used filtering algorithm is the estimation algorithm based on Kalman filter . Kalman filter is a linear filter 、 Unbiased 、 An optimal estimation algorithm based on the minimum error mean square deviation , It has precise mathematical form and excellent use efficiency . Kalman filter is essentially a data processing method , It adopts recursive filtering method , According to the measured data, a new state estimation is given by recursive equation . Due to the small amount of calculation and storage , It is easy to meet the requirements of real-time computing , It has been widely used in engineering practice .
   besides , Nonlinear filtering is also widely used in maneuvering target tracking , such as ：

Extended Kalman filter EKF
Unscented Kalman filter UKF
Volumetric Kalman filter CKF
Quadrature Kalman filter QKF
Central difference Kalman filter CDKF
Divided difference filter DDF
Gaussian mixture filter GSF
Strong tracking filtering STF
Particle filtering PF
… …

1.2. Overview of the target model

   The maneuvering target model describes the process of target state changing with time . A good model is worth a lot of data . At present, almost all target tracking algorithms are based on model for state estimation . After Kalman filter is introduced into the field of target tracking , Maneuvering target modeling based on state space has become one of the main research objects .

Common target motion models include ：

Uniform motion model ,CV
Uniform acceleration motion model ,CA
Uniform turning model ,CT
Singer Model
“ At present ” Statistical models
Jerk Model

1. The spatial motion of the target is based on different motion trajectories and coordinate systems
   One dimensional motion
   Two dimensional motion
   Three dimensional motion

2. Depending on whether the movements in different directions are related
   Uncoupled model between coordinates
   Coordinate coupling model

2. " At present " Statistics CS Model

“ At present ” The statistical model was proposed by zhouhongren in . essentially , The algorithm model is a model with non-zero mean acceleration . The algorithm considers that when the target is maneuvering at a certain acceleration , The value of acceleration at the next moment is limited , And only in “ At present ” In the neighborhood of acceleration . Of its maneuvering acceleration “ At present ” The statistical probability density is described by the modified Rayleigh distribution , The mean for “ At present ” The predicted value of acceleration .

Singer The maneuvering of the target described in the model is based on two assumptions , That is, the acceleration is zero mean
And the probability density function of acceleration is uniformly distributed .

“ At present ” Statistical models (Current Statistical Model) Is aimed at Singer The modified model proposed by these two assumptions , Make it more realistic , That is, when the target maneuvers at a certain acceleration at the current moment , According to the current statistical characteristics of maneuvering, the value of acceleration at the next moment is not random , But in a limited range, that is “ At present ” In the neighborhood of acceleration .

In this way, the model is established under two new assumptions , That is, the acceleration is non-zero mean and its probability density obeys the modified Rayleigh distribution , In the actual filtering, the predicted acceleration of the state estimation at the current time is used to replace the mean value of the acceleration .

Its acceleration conforms to the non-zero mean first-order time-dependent Markov process , namely

$$\begin{gathered} \ddot{x}(t)=\bar{a}(t)+a(t) \\ \dot{a}(t)=-\alpha a(t)+w(t)\text{\hspace{1em}(1)} \end{gathered}$$

among $\bar{a}(t)$ Is the mean acceleration ,$a(t)$ Colored noise with zero mean ,$w(t)$ Yes, the mean is 0, The variance of $2\alpha {\sigma }^2$ The Gaussian white noise of .

chart ： Adjust , Learning is so tiring

3. " At present " Statistics CS Model

3.1. " At present " Statistics CS Model （ continuity ）

because “ At present ” Statistical models (Current Statistical Model) Is aimed at Singer The modified model proposed by these two assumptions ,

therefore “ At present ” The state equation of statistical model and Singer Model is similar to , Can be expressed as ：

Let the state vector be
$$X=[x,\dot{x},\ddot{x}{]}^T$$
Then the acceleration is
$$a(t)=\ddot{x}(t)$$
Continuous time Singer The model is
$$\dot{X}(t)=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\alpha \end{array}\right]X(t)+\left[\begin{array}{c}0\\ 0\\ \alpha \end{array}\right]\bar{a}(t)+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]w(t)$$

Singer The model can also be expressed as
$$\dot{X}(t)=AX(t)+B_u\bar{a}(t)+Bw(t)$$
among
$$A=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\alpha \end{array}\right],B_u=\left[\begin{array}{c}0\\ 0\\ \alpha \end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$

3.2. " At present " Statistics CS Model （ discrete ）

cycle T After sampling discretization , It is transformed into discrete-time state equation ：

$$X_{k+1}=F_k{X}_k+G_k{\bar{a}}_k+W_k\text{\hspace{1em}(2)}$$
among
$$F_k=\left[\begin{array}{ccc}1& T& (\alpha T-1+e^{-\alpha T})/{\alpha }^2\\ 0& 1& (1-e^{-\alpha T})/\alpha \\ 0& 0& -e^{-\alpha T}\end{array}\right]$$
$$G_k=\left[\begin{array}{c}(-T+\frac{\alpha T^2}{2}+\frac{1-e^{-\alpha T}}{\alpha })/\alpha \\ T-(1-e^{-\alpha T})/\alpha \\ 1-e^{-\alpha T}\end{array}\right]$$

${\bar{a}}_k$ The predicted acceleration in the state estimation at the current time can be used to replace ：
$${\bar{a}}_k={\hat{\ddot{x}}}_{k|k-1}$$

noise $W_k$ The variance of
$$Q=2\alpha {\sigma }^2\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right]$$
$Q$ Is symmetric matrix

But ${\sigma }^2$ by ：

$${\sigma }^2=\{\begin{array}{l}\frac{4-\pi }{\pi }[a_{max}-{\bar{a}}_k{]}^2,{\bar{a}}_k\ge 0\\ \frac{4-\pi }{\pi }[a_{max}+{\bar{a}}_k{]}^2,{\bar{a}}_k<0\end{array}\text{\hspace{1em}(3)}$$

about " At present " Statistical models CS, Where the matrix $F_k$ And Singer The model is the same

about " At present " Statistical models CS, Where noise variance $W_k$ with Singer The model is the same , except ${\sigma }^2$ The certainty is different

3.3. " At present " Statistics CS model analysis

“ At present ” The statistical model uses the predicted value of acceleration to adjust the process parameters adaptively in real time ${\sigma }^2$ , With strong adaptability . therefore ,“ At present ” The statistical model is better than Singer The model can reflect the change of target maneuvering acceleration more truly . meanwhile ,“ At present ” The statistical model corrects the acceleration of the model at every time of filtering , It can track the moving target with sudden change in speed . And in terms of algorithms ,“ At present ” Statistical models and Singer The model is almost identical , The only difference is that the predicted value of the current acceleration obtained in the filtering process changes the equation of motion .

“ At present ” Acceleration when the target maneuvers strongly , Ratio Singer The model has better maneuverability , That is to say, the corresponding filter based on this model has better estimation accuracy .

It has the following two characteristics ：

1. When the target is not maneuvering , That is, the theoretical acceleration tends to 0 when , Corresponding acceleration estimate ${\hat{a}}_{k-1}$, Should tend to 0, At this time, the formula （3） Calculated ${\sigma }^2$ Tend to $\frac{(4-\pi )a_{max}^2}{\pi }$. This variance is the maximum of all possible variances , Indicates that when the target is not maneuvering , Acceleration has the greatest possibility of change . This will cause the state estimation based on the model to have a relatively large variance in the case of inorganic movement .

2. When the target maneuvers maximum or minimum , That is, the theoretical acceleration is $\pm a_{max}$ when , The corresponding acceleration estimate ${\hat{a}}_{k-1}$ Should also tend to $\pm a_{max}$, here ${\sigma }^2$ The calculated value of tends to 0. This variance is the minimum of all possible variances , Indicates that when the target is in the strongest maneuvering motion , The possibility of acceleration change is minimal . This is inconsistent with the fact that the acceleration may change at any time , Thus, the corresponding estimated value has a large estimation error when the absolute value of maneuvering acceleration is large and there is a sudden change .

chart ： Adjust , Learning is so tiring

4. " At present " Statistics CS Model （ A two-dimensional ）

The two-dimensional current statistical model is similar to the one-dimensional one , Just stack the matrix diagonally

Allied , The current three-dimensional statistical model is similar .

4.1. " At present " Statistics CS Model （ continuity ）

Let the state vector be
$$X=[x,\dot{x},\ddot{x},y,\dot{y},\ddot{y}{]}^T$$
Then the acceleration is
$$a(t)=[\ddot{x}(t),\ddot{y}(t){]}^T$$

The average acceleration is ：

$$\bar{a}(t)=[{\bar{a}}_x(t),{\bar{a}}_y(t){]}^T$$

Continuous time Singer The model is

$$\dot{X}(t)=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& -\alpha & 0& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& -\alpha \end{array}\right]X(t)+\left[\begin{array}{cc}0& 0\\ 0& 0\\ \alpha & 0\\ 0& 0\\ 0& 0\\ 0& \alpha \end{array}\right]\bar{a}(t)+\left[\begin{array}{cc}0& 0\\ 0& 0\\ 1& 0\\ 0& 0\\ 0& 0\\ 0& 1\end{array}\right]w(t)$$

For convenience , Definition
$$A=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ 0& 0& -\alpha \end{array}\right],B_u=\left[\begin{array}{c}0\\ 0\\ \alpha \end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$$
Then the above formula becomes
$$\dot{X}(t)=\left[\begin{array}{cc}A& 0\\ 0& A\end{array}\right]X(t)+\left[\begin{array}{cc}{B}_u& 0\\ 0& B_u\end{array}\right]\bar{a}(t)+\left[\begin{array}{cc}B& 0\\ 0& B\end{array}\right]w(t)$$

4.2. " At present " Statistics CS Model （ discrete ）

cycle T After sampling discretization , It is transformed into discrete-time state equation ：

$$X_{k+1}=F_k{X}_k+G_k{\bar{a}}_k+W_k$$
among
$$F_k=\left[\begin{array}{cc}F& 0\\ 0& F\end{array}\right]$$

$$F=\left[\begin{array}{ccc}1& T& (\alpha T-1+e^{-\alpha T})/{\alpha }^2\\ 0& 1& (1-e^{-\alpha T})/\alpha \\ 0& 0& -e^{-\alpha T}\end{array}\right]$$

among
$$G_k=\left[\begin{array}{cc}G& 0\\ 0& G\end{array}\right]$$

$$G_{=}\left[\begin{array}{c}(-T+\frac{\alpha T^2}{2}+\frac{1-e^{-\alpha T}}{\alpha })/\alpha \\ T-(1-e^{-\alpha T})/\alpha \\ 1-e^{-\alpha T}\end{array}\right]$$

noise $W_k$ The variance of
$$Q_k=\left[\begin{array}{cc}Q& 0\\ 0& Q\end{array}\right]$$

$$Q=2\alpha {\sigma }^2\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right]$$
$Q$ Is symmetric matrix , And

But ${\sigma }^2$ by ：

$${\sigma }^2=\{\begin{array}{l}\frac{4-\pi }{\pi }[a_{max}-{\bar{a}}_k{]}^2,{\bar{a}}_k\ge 0\\ \frac{4-\pi }{\pi }[a_{max}+{\bar{a}}_k{]}^2,{\bar{a}}_k<0\end{array}\text{\hspace{1em}(3)}$$

If for x and y The noise variance of dimension is different , Then calculate respectively Q, Others are similar , Here it is calculated as equality

5. Current statistical model matlab Simulation

Position track ： chart 1
Velocity trajectory ： chart 2
Acceleration trajectory ： chart 3
Acceleration rate of change trajectory ： chart 4

6. Current statistics CS Model 3D target tracking

Algorithm ： Kalman filtering
Volumetric Kalman filter
Unscented Kalman filter

measurement ： radar

The track is as follows ：

7. Other models

7.1 Uniform turning CT Model

Uniform turning CT See another blog for the motion model ： Including 2D 、 The three dimensional

7.2. Speed up the movement CA Model

Speed up the movement CA See another blog for the model

7.3. “ At present ” Statistical models

See another blog for the current statistical model

7.4. Singer Model

Singer See another blog for the model

8. Complete code ： Contact the beginning WX

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% Track track 
figure
plot3(sV(1,:,1,1),sV(4,:,1,1),sV(7,:,1,1),'.-k',eV(1,:,1,1),eV(4,:,1,1),eV(7,:,1,1),'--r',eV(1,:,1,2),eV(4,:,1,2),eV(7,:,1,2),'-.b','LineWidth',1)
xlabel('X(m)');ylabel('Y(m)');zlabel('Z(m)');
grid minor;
    box on;
legend(' The real track ',' Unscented Kalman filter  ',' Volumetric Kalman filter  ')
title(' Current statistical model 3D Maneuvering target tracking ')

%%%%%%%%%% Every dimension （X Y Z） Track track 
ii=1:N; % 1 3 5 It's the location ,2 4 5  It means speed 
%%%%%%%% X  Track track 
figure
plot(ii,sV(1,ii,1,1),'.-k',ii,meaV(1,ii,1),'-g',ii,eV(1,ii,1,1),'--r',ii,eV(1,ii,1,2),'-.b','LineWidth',1)
xlabel(' Time (s)');
ylabel('m');
legend(' The real track ',' Radar measurement ',' Unscented Kalman filter  ',' Volumetric Kalman filter  ')
title(' Track track ：X  dimension ')
%%%%%%%% Y  Track track 
figure
ii=1:N;
plot(ii,sV(4,ii,1,1),'.-K',ii,meaV(2,ii,1),'-g',ii,eV(4,ii,1,1),'--r',ii,eV(4,ii,1,2),'-.b','LineWidth',1)
%plot3(sV(1,:,1,1),sV(3,:,1,1),sV(5,:,1,1),'g',xV(1,:,1,1),xV(3,:,1,1),xV(5,:,1,1),'b')
xlabel(' Time (s)');
ylabel('m');
legend(' The real track ',' Radar measurement ',' Unscented Kalman filter  ',' Volumetric Kalman filter  ')
title(' Track track ：Z  dimension ')
%%%%%%%% Z  Track track 
figure
ii=1:N;
plot(ii,sV(7,ii,1,1),'.-K',ii,meaV(3,ii,1),'-g',ii,eV(7,ii,1,1),'--r',ii,eV(7,ii,1,2),'-.b','LineWidth',1)
%plot3(sV(1,:,1,1),sV(3,:,1,1),sV(5,:,1,1),'g',xV(1,:,1,1),xV(3,:,1,1),xV(5,:,1,1),'b')
xlabel(' Time (s)');
ylabel('m');
legend(' The real track ',' Radar measurement ',' Unscented Kalman filter  ',' Volumetric Kalman filter  ')
title(' Track track ：Z  dimension ')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% %%% draw RMSE
for i=1:MC
    sprintf('rate of process:%3.1f%%',(3*MC+i)/(4*MC)*100)
    for k=1:N
        for c=1:chan
            error(:,c)=sV(:,k,i,1)-eV(:,k,i,c); 
            % RMSE
            error2(:,c)=error(:,c).^2;               
            error2_dis(c)=error2(1,c)+error2(4,c)+error2(7,c);
            error2_vel(c)=error2(2,c)+error2(5,c)+error2(8,c);
            position(k,i,c)=error2_dis(c);     
            velocity(k,i,c)=error2_vel(c); 
            
        end
    end
end
%% RMSE
for c=1:chan
    rms_position(:,c)=sqrt(sum(position(:,:,c),2)./MC);  
    rms_velocity(:,c)=sqrt(sum(velocity(:,:,c),2)./MC);  
end

ii=1:N;
figure;%position
plot(ii,rms_position(ii,1),'-*r',ii,rms_position(ii,2),'-cs','LineWidth',1);
% legend('EKF','UF')
legend(' Unscented Kalman filter  ',' Volumetric Kalman filter  ')
xlabel('t/s');ylabel('Position RMSE');


figure;% Speed 
plot(ii,rms_velocity(ii,1),'-*r',ii,rms_velocity(ii,2),'-cs','LineWidth',1);
legend(' Unscented Kalman filter  ',' Volumetric Kalman filter  ')
xlabel('t/s');ylabel('Velocity RMSE');

== Originality is not easy. , Please give a compliment to all the big guys passing by =