尝试使用强跟踪EKF进行基于距离的目标追踪（matlab)

在进行IMU9轴融合时候，复现了下《基于四元数 ＥＫＦ算法的小型无人机姿态估计》发现其对于航向突变时候，无法有效收敛，便想着加入强跟踪滤波改善这种现状。学习强跟踪算法时候进行了一些尝试。
以黄小平老师的《卡尔曼滤波原理及应用 MATLAB仿真》第4.3.3节代码和强跟踪卡尔曼滤波STF估算车辆质量——matab simulink仿真进行了一些尝试，效果还行，便记录下，后续将加入EKF9轴融合算法，并落地试验下实际效果。下面是具体的代码和一些理解：
强跟踪原理：

clc;clear;close all;
fs=1;
T=1/fs;%雷达扫描周期
N=200/T;%总的采样次数
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[-100,2,200,20];%目标初始位置、速度
Z=zeros(1,N);%传感器对位置的观测
delta_w=1e-3;%如果增大这个参数,目标真实轨迹就是曲线了
Q=delta_w*diag([0.5,1]);%过程噪声方差
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
R=5;%观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
x0=200;%观测站的位置,可以设为其他值
y0=300;
Xstation=[x0,y0];

%%%%%%%%%%%%%%%%%%%%%%%%%%进行数据仿真%%%%%%%%%%%%%%%%%%%%%%%%%
for t=2:N 
    %目标真实轨迹
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);
end
for t=1:N
    Z(t)=Dist(X(:,t),Xstation)+sqrt(R)*randn;%对目标观测
end

%%%%%%%%%%%%%%%%%%%进行EKF滤波%%%%%%%%%%%%%%%%%%%%%
% EKF 滤波
% 进行滤波状态量的初始化
Xekf=zeros(4,N);
Xekf(:,1)=X(:,1);%Kalman滤波状态初始化
Xstekf=zeros(4,N);
Xstekf(:,1)=X(:,1);%Kalman滤波状态初始化
P0=eye(4);%协方差阵初始化
V0=0;
%为了演示突变，需要在某一段时间里加上突变量
tubian=[5;0.5;40;1];
for i=2:N 
%%%%%%%%%%%%%%%%%%%%%%%%EKF%%%%%%%%%%%%%%%
    if i>30 && i<32
        Xn=F*(Xekf(:,i-1)+tubian);%预测
    else
         Xn=F*(Xekf(:,i-1));%预测
    end
    Xn=F*(Xekf(:,i-1)+tubian);%预测
    dd=Dist(Xn,Xstation);%观测预测
    H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%即为所求一阶近似
    P1=F*P0*F'+G*Q*G';%预测误差协方差
    K=P1*H'*inv(H*P1*H'+R);%增益
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新


%%%%%%%%%%%%%%%%%%%强跟踪STEKF%%%%%%%%%%%
    
    if i>30 && i<32
        Xn=F*(Xstekf(:,i-1)+tubian);%预测
    else
         Xn=F*(Xstekf(:,i-1));%预测
    end
    dd=Dist(Xn,Xstation);%观测预测
    %求雅可比矩阵H 
    H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%即为所求一阶近似
    %%%%%%%%%%%%%%%%%%%%%%%强跟踪滤波器核心代码%%%%%%%%%%%%%%%%%%%%%%%%%
    rho=0.95;%残差方差阵的遗忘因子初始值(0.95<rho<0.995)
    beta=3;%量测噪声的衰减因子选定值(beta>=1)
    %beta_upmax=8;%衰减因子选定极限
    ro=Z-H*Xn;
    if i<2*T
        V0=ro*ro';
    else
        v0=(rho*V0+ro*ro')/(1+rho);
    end
    N_=V0-beta*R-H*G*Q*G'*H';
    M=H*F*F'*H';
    eta=trace(N_)/trace(M);
    if eta>1
        lamda=eta;
    else
        lamda=1;
    end
    lamda
    P1=lamda*F*P0*F'+G*Q*G';%预测误差协方差
    K=P1*H'*inv(H*P1*H'+R);%增益
    Xstekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end

%误差分析
for i=1:N 
    Err_ekf(i)=Dist(X(:,i),Xstekf(:,i));%滤波后的误差
    Err_stekf(i)=Dist(X(:,i),Xekf(:,i));%滤波后的误差
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%画图
figure 
hold on;box on;
plot(X(1,:),X(3,:),'-k.',LineWidth=1.5);%真实轨迹
plot(Xstekf(1,:),Xstekf(3,:),'-r+',Xekf(1,:),Xekf(3,:),'-b+');%扩展 Kalman 滤波轨迹
legend('真实轨迹','STEKF 轨迹','EKF轨迹');
xlabel('横坐标 X/m');
ylabel('纵坐标 Y/m');
figure; 
hold on;
box on;
plot(Err_stekf,'-ks','MarkerFace','r');
plot(Err_ekf,'-bs','MarkerFace','b');
legend('ErrStekf','Errekf');
xlabel('时间/s');
ylabel('位置估计偏差/m');
title('误差分析')

%%%%%%%%%子函数
function d=Dist(X1,X2)
    if length(X2)<=2
        d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
    else 
        d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
    end
end

运行效果图：

就是随手的记录下手边的东西，想到啥写啥。卡尔曼滤波是个很神奇的东西，初看时候不知道咋入手，上手后越发觉得奇妙。
ps:推荐一个图片文字提取的好东西，在我敲上面黄老师的代码时候帮了我大忙：

通过F4一键框选提取文字的图片部分，然后框选识别，可以在出现的类似文本框部分的空白部分右键选择”接口-》有道“解决无法识别问题，在”转换-》英文标点“解决中文字符问题，这样就可以很好的在pdf中快速提取代码。当然，还可以结合snipaste这个软件进行贴图，都是相当nice的工具。[软件下载地址]