卡尔曼滤波（3）-------目标跟踪之协同转弯或者坐标转弯模型（CT model）

基本原理

状态方程

      式中，状态矢量Xn为5*1的矢量。分别为X方向的位置，X方向的速度，Y方向的位置，Y方向的速度，转弯率。状态转移矩阵F为

      w指的是状态噪声。其来源在于由于我们所建立的物理模型和实际的目标运动轨迹有一定的误差，这部分误差我们用w表示，由于w是一个随机量，所以一般我们用它的统计量来描述。在这里我们用w的协方差矩阵Q来描述。如果Q的取值越小，则认为我们所建立的系统模型和实际的目标运动轨迹越接近，即此时相比于测量值，我们更信赖于系统模型得到的预测值。
      需要注意的是，Q的取值不是越小越好。原因有两点：1、动态模型和目标的实际运动轨迹的匹配程度；2、目标是否为非线性运动。这一点在后面的仿真中，我们也会加以说明。
       除此之外，状态噪声w更准确的表达为gamma乘以w。过程噪声输入矩阵gamma的表达式为

测量方程

测量矢量Zn为2*1的矢量。分别为X方向的位置，Y方向的位置。
测量矩阵H为

源代码

产生目标的运动轨迹

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      设定目标运动轨迹
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X=[];           % 目标水平方向的真实位置、速度;垂直方向的的真实位置
r=[];           % 目标与原点的距离
X0 = 1000;
Y0 = 5000;
Vx = 300;
Vy = 200;
womiga=3;   % 转弯率
Y(:,1)=[X0,Vx,Y0,Vy,womiga];      % 目标初始位置为（100,100）、初始速度为（200,100） x,vx,y,vy
F=[1,sind(womiga*T)/womiga,0,(cosd(womiga*T)-1)/womiga,0;...
   0,cosd(womiga*T),       0,-sind(womiga*T),0;...
   0,(1-cosd(womiga*T))/womiga,1,sind(womiga*T)/womiga,0;...
   0,sind(womiga*T),0,cosd(womiga*T),0;...
   0,0,0,0,1];% 状态转移矩阵
w=sqrt(Q)*randn(5,M);  % 在匀速运动轨迹上叠加噪声
for p=2:M
    Y(:,p)=F*Y(:,p-1)+w(:,p);   % 目标运动轨迹方程
%     Y(:,p)=F*Y(:,p-1);            % 目标运动轨迹方程
    r(p)=sqrt(Y(1,p)^2+Y(3,p)^2);
    theta_true(p)=acos(Y(3,p)/r(p))*180/pi;
end
% 循环从2开始，所以需要下面两行代码
r(1)=sqrt(Y(1,1)^2+Y(3,1)^2);
theta_true(1)=acos(Y(3,1)/r(1))*180/pi;  % 将位置换成角度

kalman 滤波函数

function [X_est,X_pre,P_pre] = KEM_tracking(X_pre,P_pre,theta_m,F,Q,Error_est,theta_true)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   函数功能：卡尔曼滤波      更新估计、预测
%   输入参数：状态预测值，估计误差预测值，目标捕获得到的角度,状态转移矩阵，状态噪声方差，测量噪声方差
%   输出参数：卡尔曼滤波的角度thetas_est，下一时刻的预测值，估计误差的预测值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% 观测方程的系数矩阵
H=[1,0,0,0,0;0,0,1,0,0];       
% 观测值
Z=[sind(theta_m) cosd(theta_m)]'; 

%测量误差
% T = [cosd(theta_true) -sind(theta_true)]';
% R = T*[Error_est]*T';
R =Error_est*eye(2); 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     开始迭代过程（第一次迭代等等）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 更新
a=Z -H*X_pre;           % 新息计算
A=H*P_pre*H'+R;        % 新息过程的自相关矩阵
K=P_pre*H'*inv(A);      % 计算卡尔曼增益

X_est=X_pre+K*a;   % 状态估计
P_est=(eye(5)-K*H)*P_pre*(eye(5)-K*H)'+K*R*K'; % 估计误差自相关矩阵


% 预测
X_pre = F*X_est;    % 下一时刻状态的预测值  相当于X（n+1,n）
P_pre = F*P_est*F'+Q;             % 预测状态误差自相关矩阵  相当于P（n+1,n）
    
    

end

仿真结果

目标的运动轨迹

目标角度的运动轨迹

kalman 滤波结果
当Q=eye（3）时

当Q=0.01*eye（3）时