Suivi des cibles de manoeuvre - - mise en oeuvre du modèle statistique actuel (modèle CS) filtre Kalman étendu / filtre Kalman sans trace par MATLAB

Manoeuvre de suivi des cibles——Modèle statistique actuel（CSModèle）Filtre Kalman étendu/Filtre Kalman sans trace matlabRéalisation

L'originalité n'est pas facile,Les gars qui passent, s'il vous plaît

Discussion sur le suivi des cibles de manoeuvre、Support technique bienvenue contact,Ou un message privé
WX: ZB823618313

Fig.：Réglage, Je suis si fatigué d'étudier.

1. Compréhension du suivi des cibles de manoeuvre

1.1. Compréhension du suivi des cibles de manoeuvre

   Le suivi mobile des cibles est toujours un problème difficile et important dans le domaine du suivi des cibles. . L'établissement d'un modèle de mouvement de la cible et d'un algorithme de filtrage sont deux facteurs importants pour le suivi de la cible. . En raison de l'imprévisibilité de la manœuvre de la cible , Il est difficile de modéliser avec précision le mouvement de la cible . Comment construire un modèle efficace pour refléter la trajectoire réelle de la cible est un problème urgent pour le système de suivi des cibles à haute manoeuvre . Après près de 30 ans de recherche , De nombreux résultats importants ont été obtenus dans ce domaine. .

Compréhension personnelle： Le suivi des cibles de manoeuvre comporte trois éléments: ：

Modélisation des cibles suivies （ Ce blog se concentre sur ：Modèle statistique actuel）
Mesure du capteur（ Une autre présentation de blog ）
Conception du filtre（ Voir la colonne de suivi des cibles ）

  Au niveau de l'algorithme, Dans le système de suivi des cibles , L'algorithme de filtrage commun est un algorithme d'estimation basé sur le filtre Kalman . Le filtre Kalman est une sorte de linéarité 、Non biaisé、 Algorithme d'estimation optimal basé sur la variance moyenne minimale de l'erreur , Il a une forme mathématique précise et une excellente efficacité d'utilisation . La méthode de filtrage Kalman est essentiellement une méthode de traitement des données , Il utilise un filtre récursif , Une nouvelle estimation de l'état est dérivée Récursivement de l'équation récursive à partir des données de mesure obtenues. . En raison de la petite quantité de calcul et de stockage , Il est facile de répondre aux exigences du calcul en temps réel , Largement utilisé dans la pratique de l'ingénierie .
  En plus de ça,, Le filtrage non linéaire est également largement utilisé dans le suivi des cibles de manoeuvre ,Par exemple,：

Filtre Kalman étenduEKF
Filtre Kalman sans traceUKF
Filtre Kalman volumétriqueCKF
Filtre Kalman QuadratureQKF
Filtre Kalman différentiel centralCDKF
Divided difference filter DDF
Filtre de mélange gaussienGSF
Filtre de suivi fortSTF
Filtre à particulesPF
… …

1.2. Aperçu du modèle cible

   Le modèle de cible mobile décrit le processus par lequel l'état de la cible change avec le temps . Un bon modèle vaut beaucoup de données. . À l'heure actuelle, presque tous les algorithmes de suivi des cibles sont basés sur des modèles pour estimer l'état. . Après l'introduction du filtre Kalman dans le domaine du suivi des cibles , La modélisation des cibles de manoeuvre basée sur l'espace d'état est devenue l'un des principaux objets de recherche .

Les modèles de mouvement des cibles couramment utilisés comprennent: ：

Modèle de mouvement uniforme,CV
Modèle de mouvement accéléré uniforme ,CA
Modèle de virage à vitesse constante ,CT
Singer Modèle
“En cours”Modèle statistique
Jerk Modèle

1. Le Mouvement spatial de la cible est basé sur différents trajectoires de mouvement et systèmes de coordonnées
   Mouvement unidimensionnel
   Mouvement bidimensionnel
   Mouvement 3D

2. Selon que le mouvement dans différentes directions est corrélé ou non
   Modèle de découplage entre coordonnées
   Modèle de couplage entre coordonnées

2. "En cours"StatistiquesCSModèle

“En cours” Le modèle statistique a été proposé par Zhou Hongren en .Essentiellement, Le modèle algorithmique est un modèle d'accélération avec une moyenne non nulle . L'algorithme suppose que lorsque la cible se déplace à une certaine accélération , L'accélération du moment suivant est limitée ,Et seulement dans“En cours” Dans le voisinage de l'accélération . Dont l'accélération de la manœuvre “En cours” .La densité de probabilité statistique est décrite à l'aide d'une distribution de Rayleigh modifiée ,La moyenne est“En cours” Valeur prévue de l'accélération .

Singer La manoeuvre des cibles décrites dans le modèle repose sur deux hypothèses , C'est - à - dire que l'accélération est une moyenne nulle
Et la fonction de densité de probabilité de l'accélération suit une distribution uniforme .

“En cours”Modèle statistique(Current Statistical Model)Oui. Singer Le modèle modifié proposé par ces deux hypothèses , Pour le rendre plus réaliste , C'est - à - dire que la cible est censée manoeuvrer à une certaine accélération à l'heure actuelle , En utilisant les caractéristiques statistiques actuelles de la manœuvre, nous pouvons savoir que la valeur de l'accélération au moment suivant n'est pas aléatoire. , Mais dans une certaine mesure, “En cours” Dans le voisinage de l'accélération .

De cette façon, le modèle construit deux nouvelles hypothèses sous , C'est - à - dire que l'accélération n'est pas une moyenne nulle et que sa densité de probabilité suit une distribution de Rayleigh modifiée. , La valeur moyenne de l'accélération est remplacée par l'accélération prédite de l'estimation de l'état au moment actuel dans le filtrage réel. .

Son accélération correspond à un processus de Markov dépendant du temps de premier ordre avec une moyenne non nulle ,C'est - à - dire:

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

Parmi eux$\bar{a}(t)$ Est la valeur moyenne de l'accélération ,$a(t)$ Bruit coloré à moyenne nulle ,$w(t)$ Oui, la moyenne est 0,La variance est$2\alpha {\sigma }^2$Le bruit blanc gaussien de.

Fig.：Réglage, Je suis si fatigué d'étudier.

3. "En cours"StatistiquesCSModèle

3.1. "En cours"StatistiquesCSModèle（Continu）

Parce que“En cours”Modèle statistique(Current Statistical Model)Oui. Singer Le modèle modifié proposé par ces deux hypothèses ,

Donc, “En cours” L'équation d'état du modèle statistique et Singer Modèle similaire,Peut être exprimé comme suit:：

Faire le vecteur d'état
$$X=[x,\dot{x},\ddot{x}{]}^T$$
Alors l'accélération est
$$a(t)=\ddot{x}(t)$$
Temps continuSingerLe modèle est
$$\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 Le modèle peut également être exprimé comme suit:
$$\dot{X}(t)=AX(t)+B_u\bar{a}(t)+Bw(t)$$
Parmi eux
$$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. "En cours"StatistiquesCSModèle（Discrete）

CycleT Après la discrétisation de l'échantillon , Convertir l'équation d'état en temps discret en ：

$$X_{k+1}=F_k{X}_k+G_k{\bar{a}}_k+W_k\text{\hspace{1em}(2)}$$
Parmi eux
$$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$ Il peut être remplacé par l'accélération prévue dans l'estimation de l'état au moment actuel ：
$${\bar{a}}_k={\hat{\ddot{x}}}_{k|k-1}$$

Bruit$W_k$La variance de
$$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$Pour une matrice symétrique

Parmi eux, mais ${\sigma }^2$Pour：

$${\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)}$$

Pour"En cours"Modèle statistiqueCS,Où matrice$F_k$AvecSingerLe même modèle.

Pour"En cours"Modèle statistiqueCS, Où variance du bruit $W_k$EtSingerLe même modèle.,Sauf que${\sigma }^2$ C'est différent.

3.3. "En cours"StatistiquesCSAnalyse du modèle

“En cours” Le modèle statistique utilise la valeur prédictive de l'accélération pour ajuster les paramètres du processus de façon adaptative en temps réel ${\sigma }^2$ , Avec une grande adaptabilité .Donc,,“En cours” Modèle statistique comparé à Singer Le modèle peut refléter des changements plus réalistes dans l'accélération de la manœuvre de la cible .En même temps,“En cours” Le modèle statistique modifie l'accélération du modèle à chaque moment du filtrage , Il est possible de suivre très bien les cibles qui se déplacent à des vitesses soudaines . Et en termes d'algorithmes, ,“En cours” Modèles statistiques et Singer Le modèle est presque identique , La seule différence est que l'équation de l'état du Mouvement est modifiée par la valeur prédite de l'accélération actuelle obtenue au cours du filtrage. .

“En cours” Accélération en cas de forte manœuvre de la cible ,A un rapport Singer Une meilleure description de la mobilité du modèle , C'est - à - dire que les filtres correspondants basés sur ce modèle ont une meilleure précision d'estimation .

Il présente les deux caractéristiques suivantes: ：

1. Lorsque la cible n'a pas de manoeuvre , C'est - à - dire que l'accélération théorique tend à 0Heure, Estimation de l'accélération correspondante ${\hat{a}}_{k-1}$, Devrait avoir tendance à 0, Pour le moment （3）Calculé${\sigma }^2$Tendance$\frac{(4-\pi )a_{max}^2}{\pi }$. Cette variance est la valeur maximale de toutes les variances possibles , Indique que lorsque la cible n'a pas de manoeuvre , L'accélération a la plus grande probabilité de changement . Il en résulte une grande variance des estimations de l'état basées sur le modèle lorsqu'elles sont inorganiques. .

2. Lorsque la manœuvre de la cible est maximale ou minimale , C'est - à - dire que l'accélération théorique est $\pm a_{max}$Heure, Estimation de l'accélération correspondante ${\hat{a}}_{k-1}$ Devrait également tendre vers $\pm a_{max}$,En ce moment${\sigma }^2$ La valeur calculée de 0. Cette variance est la valeur minimale de toutes les variances possibles , Indique que lorsque la cible effectue le mouvement de manoeuvre le plus fort , Possibilité minimale de changement d'accélération . Cela ne correspond pas au fait que l'accélération peut changer à tout moment. , Par conséquent, l'estimation correspondante a une grande erreur d'estimation lorsque la valeur absolue de l'accélération de manoeuvre est grande et qu'un changement soudain se produit. .

Fig.：Réglage, Je suis si fatigué d'étudier.

4. "En cours"StatistiquesCSModèle（2D）

Le modèle statistique actuel en 2D est similaire à celui en 1D , Il suffit d'empiler la matrice en diagonale

Similaire, Le modèle statistique actuel en 3D est similaire .

4.1. "En cours"StatistiquesCSModèle（Continu）

Faire le vecteur d'état
$$X=[x,\dot{x},\ddot{x},y,\dot{y},\ddot{y}{]}^T$$
Alors l'accélération est
$$a(t)=[\ddot{x}(t),\ddot{y}(t){]}^T$$

La valeur moyenne de l'accélération est ：

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

Temps continuSingerLe modèle est

$$\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)$$

Pour plus de commodité,Définition
$$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]$$
Alors la formule ci - dessus devient
$$\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. "En cours"StatistiquesCSModèle（Discrete）

CycleT Après la discrétisation de l'échantillon , Convertir l'équation d'état en temps discret en ：

$$X_{k+1}=F_k{X}_k+G_k{\bar{a}}_k+W_k$$
Parmi eux
$$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]$$

Parmi eux
$$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]$$

Bruit$W_k$La variance de
$$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$Pour une matrice symétrique,Et

Parmi eux, mais ${\sigma }^2$Pour：

$${\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)}$$

Six Et y La variance du bruit des dimensions est différente ,Calculer séparémentQ,Autres similaires, C'est un calcul d'égalité.

5. Modèle statistique actuelmatlabSimulation

Trajectoire de position： Fig.1
Trajectoire de vitesse： Fig.2
Trajectoire d'accélération ： Fig.3
Trajectoire du taux de changement d'accélération ： Fig.4

6. Statistiques actuellesCSModèle 3D suivi des cibles

Algorithmes：Filtre Kalman
Filtre Kalman volumétrique
Filtre Kalman sans trace

Mesure：Radar

La piste est la suivante: ：

7. Autres modèles

7.1 Tourner à vitesse constante CTModèle

Tourner à vitesse constante CT Modèle de mouvement voir un autre blog ：Y compris 2D、3D

7.2. Accélération uniforme du MouvementCAModèle

Accélération uniforme du MouvementCA Modèle voir un autre blog

7.3. “En cours”Modèle statistique

Voir un autre blog pour le modèle statistique actuel

7.4. SingerModèle

Singer Modèle voir un autre blog

8. Code complet： Début du contact WX

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% Tracez la piste 
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('Trajectoire réelle','Filtre Kalman sans trace ','Filtre Kalman volumétrique ')
title('Modèle statistique actuel3DManoeuvre de suivi des cibles')

%%%%%%%%%%Dimensions individuelles（X Y Z） Tracez la piste 
ii=1:N; % 1 3 5Se réfère à la position,2 4 5  C'est la vitesse. 
%%%%%%%% X  Tracez la piste 
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('Temps(s)');
ylabel('m');
legend('Trajectoire réelle',' Mesures radar ','Filtre Kalman sans trace ','Filtre Kalman volumétrique ')
title(' Tracez la piste ：X Vi')
%%%%%%%% Y  Tracez la piste 
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('Temps(s)');
ylabel('m');
legend('Trajectoire réelle',' Mesures radar ','Filtre Kalman sans trace ','Filtre Kalman volumétrique ')
title(' Tracez la piste ：Z Vi')
%%%%%%%% Z  Tracez la piste 
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('Temps(s)');
ylabel('m');
legend('Trajectoire réelle',' Mesures radar ','Filtre Kalman sans trace ','Filtre Kalman volumétrique ')
title(' Tracez la piste ：Z Vi')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% %%%La peintureRMSE
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('Filtre Kalman sans trace ','Filtre Kalman volumétrique ')
xlabel('t/s');ylabel('Position RMSE');


figure;%Vitesse
plot(ii,rms_velocity(ii,1),'-*r',ii,rms_velocity(ii,2),'-cs','LineWidth',1);
legend('Filtre Kalman sans trace ','Filtre Kalman volumétrique ')
xlabel('t/s');ylabel('Velocity RMSE');

==L'originalité n'est pas facile,Les gars qui passent, s'il vous plaît=