【滤波跟踪】基于Huber函数和最大相关熵的抗差滤波算法实现GNSS导航定位粗差处理附matlab代码

1 内容介绍

雷达系统中跟踪滤波器的设计通常依赖于线性高斯系统.一旦系统为非线性且受到非高斯噪声干扰时,雷达跟踪性能便出现严重恶化.为了提高目标在非线性非高斯环境下跟踪的精度,将最大相关熵扩展卡尔曼滤波(MCEKF)算法应用到雷达跟踪系统中.该算法使用最大相关熵(MCC)而非传统的最小均方误差(MMSE)作为优化准则,具有较强的定位精度以及鲁棒性.最后通过仿真实验与传统扩展卡尔曼滤波(EKF)算法进行了性能比较,结果表明:最大相关熵扩展卡尔曼滤波(MCEKF)算法精度高于(EKF)算法.

2 部分代码

clc;

clear;

close all;

load('data1');

% load('Samples');

%产生真实轨迹

x0=0;

vx0=0;

x(1)=x0;

vx(1)=vx0;

NS1=200;

NS2=1500;

t=1500;

for i=2:t

    x(i)=x0+vx0*(i-1);

    vx(i)=vx0;

end

Xtrue=[x;vx];

%观测模型

R=9;%观测噪声协方差

%开始MC仿真

for kk=1:100

%产生随机数过程

N=t;

U=rand(1,N);

for i=1:N

    if U(i)<0.5

        Sam(i)=randn;

    else

        Sam(i)=sqrt(5)*randn;%均值为0，方差为5

    end

end

for i=1:t

%     nx(i)=data1(i);

%     nx(i)=sqrt(R)*randn;

    nx(i)=Sam(i);

    Y(i)=x(i)+nx(i);

end

% Y(1,100)=Y(1,100)-25;

% Y(1,500)=Y(1,500)+35;

% Y(1,900)=Y(1,900)-45;

% Y(1,1300)=Y(1,1300)+55;

F = [1 1; 0 1]; % the system matrix

H = [1 0];%the observation matrix

%Q=0.1*eye(2);%过程噪声协方差

Q=zeros(2,2);

Q(1,1)=1E-2;

Q(2,2)=1E-2;

init_X=[x(1);vx(1)];

init_P=100*eye(2);

%MCC based KF

for i=1:t

    if i==1

        XhatM=init_X;

        PhatM=init_P;

        Xhat(:,i)=XhatM;

        Phat(:,:,i)=PhatM;

    else

        XbarM=F*XhatM;

        PbarM=F*PhatM*F'+Q;

        [XhatM,PhatM] = RKF_MCC (Y(i),R,H,XbarM,PbarM);

        Xhat(:,i)=XhatM;

        Phat(:,:,i)=PhatM;

    end

    errM{kk}(:,i)=Xtrue(:,i)-Xhat(:,i);

end

%Huber based KF

for i=1:t

    if i==1

        XhatH=init_X;

        PhatH=init_P;

        Xhat(:,i)=XhatH;

        Phat(:,:,i)=PhatH;

    else

        XbarH=F*XhatH;

        PbarH=F*PhatH*F'+Q;

        [XhatH,PhatH] = RKF_Huber (Y(i),R,H,XbarH,PbarH);

        Xhat(:,i)=XhatH;

        Phat(:,:,i)=PhatH;

    end

    errH{kk}(:,i)=Xtrue(:,i)-Xhat(:,i);

end

%Classical KF

for i=1:t

    if i==1

        xhat=init_X;

        phat=init_P;

        XhatK(:,i)=xhat;

        PhatK(:,:,i)=phat;

    else

        xbar=F*xhat;       

        pbar=F*phat*F'+Q;

        [xhat,phat] = Kalman (Y(i),H,xbar,pbar,R);

        XhatK(:,i)=xhat;

        PhatK(:,:,i)=phat;

    end

    errK{kk}(:,i)=Xtrue(:,i)-XhatK(:,i);

end

end

for i=1:100

    ppK(i,:)=errK{i}(1,:);

    ppH(i,:)=errH{i}(1,:);

    ppM(i,:)=errM{i}(1,:);

end

for i=1:100

    vvK(i,:)=errK{i}(2,:);

    vvH(i,:)=errH{i}(2,:);

    vvM(i,:)=errM{i}(2,:);

end

for j=1:t

    PstdK(j)=std(ppK(:,j));

    PstdH(j)=std(ppH(:,j));

    PstdM(j)=std(ppM(:,j));

end

for j=1:t

    VstdK(j)=std(vvK(:,j));

    VstdH(j)=std(vvH(:,j));

    VstdM(j)=std(vvM(:,j));

end

figure(1)

subplot(2,1,1)

plot(1:t,PstdM(1,:),'r',1:t,PstdK(1,:),'b',1:t,PstdH(1,:),'g');

legend('MKF','CKF','HKF')

subplot(2,1,2)

plot(1:t,VstdM(1,:),'r',1:t,VstdK(1,:),'b',1:t,VstdH(1,:),'g');

% figure(1)

% subplot(2,1,1)

% plot(1:t,errM(1,:),'r',1:t,errK(1,:),'b',1:t,errH(1,:),'g');

% %plot(1:t,errR(1,:),'r',1:t,errK(1,:),'b');

% legend('MKF','CKF','HKF')

% title('位置估计误差');

% subplot(2,1,2)

% plot(1:t,errM(2,:),'r',1:t,errK(2,:),'b',1:t,errH(2,:),'g');

% title('速度估计误差');

3 运行结果

4 参考文献

​[1]王恒, 李春霞, 刘守训. 基于最大相关熵的雷达扩展卡尔曼滤波算法研究[J]. 中国传媒大学学报：自然科学版, 2020, 27(3):5.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机、雷达通信、无线传感器等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。