⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 npfmain.m

📁 文件夹中NPFMain.m为滤波算法主运行程序
💻 M
字号:
🔍 
%Copyright (c) 2005 by Research Institute of Electronic Engineering in HIT
%All rights reserved
%BTPFMain.m
%Abstract: Nonlinear State Model and Nonlinear Measure Model
%Author: DingZhang Dai
tic,
clc;
clear all;
close all;
load('RealStateNonGaussianNoManeu.mat');
NGDen=0.05; %NGDen代表观测噪声的非高斯程度
PtAcc=60;   %跟踪的点数
MCAcc=1;  %设定蒙特卡罗仿真次数
Ns=100;    %设定本程序中粒子滤波的粒子总数

%状态方程和观测方程中各矩阵和参数的定义
global T Phi PhiE G GE Beta g q H Q;
T=2;  %设定数据更新时间间隔
Phi=[1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1]; %定义了状态转移矩阵
PhiE=[1 T 0 0 0; 0 1 0 0 0; 0 0 1 T 0; 0 0 0 1 0; 0 0 0 0 1]; %扩展状态转移矩阵
G=[T^2/2, T, 0, 0; 0, 0, T^2/2, T]'; 
GE=[T^2/2, T, 0, 0, 0; 0, 0, T^2/2, T, 0]';
H=[1 0 0 0; 0 0 1 0];
HE=[1 0 0 0 0; 0 0 1 0 0];
Beta=40000; %定义了真实的弹道系数
g=9.81; %定义了重力加速度
q=5; %定义了过程噪声强度
SigmaR=150; %径向距离观测误差均方差
SigmaA=0.017; %角度观测误差均方差
SigmaBeta=5000; %弹道系数估计的误差均方差
EstBeta=Beta+normrnd(0,SigmaBeta);
RPolar=[SigmaR^2, 0;
    0,       SigmaA^2]; %观测误差协方差矩阵

Q=q*[T^3/3, T^2/2, 0,       0,     0;
     T^2/2, T,     0,       0,     0;
     0,     0,     T^3/3,   T^2/2, 0;
     0,     0,     T^2/2,   T,     0;
     0,     0,     0,       0,     T]; %系统噪声协方差矩阵
%状态方程和观测方程中各矩阵和参数定义结束

%各存储矩阵的初始化
EKFFilState=zeros(5,PtAcc);       %EKF滤波值存储矩阵
MeaError=zeros(2,PtAcc,MCAcc);    %观测误差存储矩阵
MeaErrorMean=zeros(2,PtAcc);      %观测误差均值存储矩阵
MeaErrorVar=zeros(2,PtAcc);       %观测误差均方差存储矩阵
EKFFilError=zeros(5,PtAcc,MCAcc); %EKF滤波误差存储矩阵
EKFFilErrorMean=zeros(5,PtAcc);   %多次蒙特卡洛仿真EKF滤波误差均值存储矩阵
EKFFilErrorVar=zeros(5,PtAcc);    %多次蒙特卡洛仿真EKF滤波误差均方差存储矩阵
FilErrMat=zeros(5,5,PtAcc);       %EKF滤波误差协方差矩阵

Pw=zeros(Ns,PtAcc);               %权值矩阵
Particles=zeros(5,Ns,PtAcc);      %粒子存储矩阵


for MCNum=1:MCAcc
    %进行观测，前两行是径向和角度观测，后两行为转化后的直角坐标系的观测
    Observe(1,:,MCNum)=sqrt(RealState(1,:).*RealState(1,:)+RealState(3,:).*RealState(3,:))+Glint(NGDen,SigmaR,1,PtAcc);
    Observe(2,:,MCNum)=atan2(RealState(3,:),RealState(1,:))+Glint(NGDen,SigmaA,1,PtAcc);
    ObserveCar(1,:,MCNum)=Observe(1,:,MCNum).*cos(Observe(2,:,MCNum));
    ObserveCar(2,:,MCNum)=Observe(1,:,MCNum).*sin(Observe(2,:,MCNum));
    %EKF滤波
    %首先估计初始滤波点——两点法起始
    EKFFilState(1,2)=ObserveCar(1,2,MCNum);
    EKFFilState(2,2)=(ObserveCar(1,2,MCNum)-ObserveCar(1,1,MCNum))/T;
    EKFFilState(3,2)=ObserveCar(2,2,MCNum);
    EKFFilState(4,2)=(ObserveCar(2,2,MCNum)-ObserveCar(2,1,MCNum))/T;
    EKFFilState(5,2)=40000+normrnd(0,5000);
    SigmaDSquare=SigmaR^2*ObserveCar(1,2,MCNum)^2/(ObserveCar(1,2,MCNum)^2+ObserveCar(2,2,MCNum)^2)+...
                 SigmaA^2*ObserveCar(2,2,MCNum)^2;
    SigmaHSquare=SigmaR^2*ObserveCar(2,2,MCNum)^2/(ObserveCar(1,2,MCNum)^2+ObserveCar(2,2,MCNum)^2)+...
                 SigmaA^2*ObserveCar(1,2,MCNum)^2;
    SigmaDH=(SigmaR^2/(ObserveCar(1,2,MCNum)^2+ObserveCar(2,2,MCNum)^2)-SigmaA^2)*...
             ObserveCar(1,2,MCNum)*ObserveCar(2,2,MCNum);
    
    EKFFilMat(:,:,2)=1000*[SigmaDSquare,    -SigmaDSquare/T,       SigmaDH,         -SigmaDH/T,           0;
                           -SigmaDSquare/T, 2*SigmaDSquare/(T^2),  -SigmaDH/T,      2*SigmaDH/(T^2),      0;
                           SigmaDH,         -SigmaDH/T,            SigmaHSquare,    -SigmaHSquare/T,      0;
                           -SigmaDH/T,      2*SigmaDH/(T^2),       -SigmaHSquare/T, 2*SigmaHSquare/(T^2), 0;
                           0,               0,                     0,               0,                    (SigmaBeta/10)^2];
    %滤波过程
    for PtNum=3:PtAcc
        SigmaDSquare=SigmaR^2*ObserveCar(1,PtNum,MCNum)^2/(ObserveCar(1,PtNum,MCNum)^2+ObserveCar(2,PtNum,MCNum)^2)+...
                     SigmaA^2*ObserveCar(2,PtNum,MCNum)^2;
        SigmaHSquare=SigmaR^2*ObserveCar(2,PtNum,MCNum)^2/(ObserveCar(1,PtNum,MCNum)^2+ObserveCar(2,PtNum,MCNum)^2)+...
                     SigmaA^2*ObserveCar(1,PtNum,MCNum)^2;
        SigmaDH=(SigmaR^2/(ObserveCar(1,PtNum,MCNum)^2+ObserveCar(2,PtNum,MCNum)^2)-SigmaA^2)*...
                     ObserveCar(1,PtNum,MCNum)*ObserveCar(2,PtNum,MCNum);
        R=[SigmaDSquare, SigmaDH;
           SigmaDH,     SigmaHSquare]; %直角坐标系下观测误差协方差矩阵
       %求取当前时刻的雅可比矩阵
       Jacobi(1,1)=0;
       Jacobi(2,1)=0;
       Jacobi(1,2)=-0.5*g/EKFFilState(5,PtNum-1)*Rho(EKFFilState(3,PtNum-1))*(2*EKFFilState(2,PtNum-1)^2+...
           EKFFilState(4,PtNum-1)^2)/sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);
       Jacobi(2,2)=-0.5*g/EKFFilState(5,PtNum-1)*Rho(EKFFilState(3,PtNum-1))*EKFFilState(2,PtNum-1)*...
           EKFFilState(4,PtNum-1)/sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);
       Jacobi(1,3)=0.5*g/EKFFilState(5,PtNum-1)*Rho(EKFFilState(3,PtNum-1))*c2(EKFFilState(3,PtNum-1))*...
           EKFFilState(2,PtNum-1)*sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);
       Jacobi(2,3)=0.5*g/EKFFilState(5,PtNum-1)*Rho(EKFFilState(3,PtNum-1))*c2(EKFFilState(3,PtNum-1))*...
           EKFFilState(4,PtNum-1)*sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);
       Jacobi(1,4)=Jacobi(2,2);
       Jacobi(2,4)=-0.5*g/EKFFilState(5,PtNum-1)*Rho(EKFFilState(3,PtNum-1))*(2*EKFFilState(4,PtNum-1)^2+...
           EKFFilState(2,PtNum-1)^2)/sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);
       Jacobi(1,5)=0.5*g/(EKFFilState(5,PtNum-1)^2)*Rho(EKFFilState(3,PtNum-1))*...
           EKFFilState(2,PtNum-1)*sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);
       Jacobi(2,5)=0.5*g/(EKFFilState(5,PtNum-1)^2)*Rho(EKFFilState(3,PtNum-1))*...
           EKFFilState(4,PtNum-1)*sqrt(EKFFilState(2,PtNum-1)^2+EKFFilState(4,PtNum-1)^2);

       PreState=PhiE*EKFFilState(:,PtNum-1)+GE*DragAcceE(EKFFilState(:,PtNum-1))+GE*[0, -g]'; %状态预测
       PreErrMtr=(PhiE+GE*Jacobi)*EKFFilMat(:,:,PtNum-1)*(PhiE+GE*Jacobi)'+Q;                 %状态预测误差协方差
       Gain=PreErrMtr*HE'*inv(HE*PreErrMtr*HE'+R);                                            %增益矩阵
       EKFFilState(:,PtNum)=PreState+Gain*(ObserveCar(:,PtNum,MCNum)-HE*PreState);            %状态滤波
       EKFFilMat(:,:,PtNum)=(eye(5)- Gain*HE)*PreErrMtr;                                      %状态误差协方差
    end
    EKFFilError(1:4,:,MCNum)=EKFFilState(1:4,:)-RealState(:,:);
    EKFFilError(5,:,MCNum)=EKFFilState(5,:)-40000;
    


%NPF滤波
    ObsInCart(1,:)=Observe(1,:,MCNum).*cos(Observe(2,:,MCNum));
    ObsInCart(2,:)=Observe(1,:,MCNum).*sin(Observe(2,:,MCNum));
    
    NPFFilState(1,1)=Observe(1,1,MCNum)*cos(Observe(2,1,MCNum));
    NPFFilState(3,1)=Observe(1,1,MCNum)*sin(Observe(2,1,MCNum));
    
    NPFFilState(1,2)=Observe(1,2,MCNum)*cos(Observe(2,2,MCNum));
    NPFFilState(2,2)=(NPFFilState(1,2)-NPFFilState(1,1))/T;
    NPFFilState(3,2)=Observe(1,2,MCNum)*sin(Observe(2,2,MCNum));
    NPFFilState(4,2)=(NPFFilState(3,2)-NPFFilState(3,1))/T;
    NPFFilState(5,2)=EstBeta;
    %%粒子初始化
    ParR2=Observe(1,2,MCNum)+Glint(NGDen,SigmaR,1,Ns);
    ParA2=Observe(2,2,MCNum)+Glint(NGDen,SigmaA,1,Ns);
    ParR1=Observe(1,1,MCNum)+Glint(NGDen,SigmaR,1,Ns);
    ParA1=Observe(2,1,MCNum)+Glint(NGDen,SigmaA,1,Ns);
    Particles(1,:,2)=ParR2.*cos(ParA2);
    Particles(2,:,2)=(ParR2.*cos(ParA2)-ParR1.*cos(ParA1))/T;
    Particles(3,:,2)=ParR2.*sin(ParA2);
    Particles(4,:,2)=(ParR2.*sin(ParA2)-ParR1.*sin(ParA1))/T;
    Particles(5,:,2)=EstBeta+normrnd(0,SigmaBeta,1,Ns);
    Pw(:,2)=1/Ns;          %定义初始权值
    %%粒子初始化完毕
    %%开始进行粒子滤波
    for PtNum=3:PtAcc
        if PtNum>3
            Particles(:,:,PtNum-1)=NPFFilState(:,PtNum-1)*ones(1,Ns)+Decompose(FilErrMat(:,:,PtNum-1))*normrnd(0,1,5,Ns);%Glint(0.2,1,5,Ns);            
        end
        for ParNum=1:Ns
            w(:,ParNum)=Decompose(Q)*normrnd(0,1,5,1);
            Particles(:,ParNum,PtNum)=PhiE*Particles(:,ParNum,PtNum-1)+GE*...
                    DragAcceE(Particles(:,ParNum,PtNum-1))+GE*[0,-g]'+w(:,ParNum);             %更新粒子
            Temp1=Observe(:,PtNum,MCNum)-hnonl(Particles(:,ParNum,PtNum));
            Pw(ParNum,PtNum)=((1-NGDen)/sqrt(2*pi*RPolar(1,1))*exp(-0.5*Temp1(1)^2/RPolar(1,1))+NGDen/(6*SigmaR)*exp(-abs(Temp1(1))/(3*SigmaR)))*...
                ((1-NGDen)/sqrt(2*pi*RPolar(2,2))*exp(-0.5*Temp1(2)^2/RPolar(2,2))+NGDen/(6*SigmaA)*exp(-abs(Temp1(2))/(3*SigmaA)));
        end
        Pw(:,PtNum)=Pw(:,PtNum)/sum(Pw(:,PtNum));
        NPFFilState(:,PtNum)=Particles(:,:,PtNum)*Pw(:,PtNum); %输出滤波值
        FilErrMat(:,:,PtNum)=WeightedCovar(NPFFilState(:,PtNum),Particles(:,:,PtNum),Pw(:,PtNum));%输出滤波误差协方差矩阵
        Process=(PtNum+(MCNum-1)*PtAcc)/(PtAcc*MCAcc), %在控制台上提示程序的运行进度
    end
    NPFFilError(1:4,:,MCNum)=NPFFilState(1:4,:)-RealState(:,:);
    NPFFilError(5,:,MCNum)=NPFFilState(5,:)-40000;
    MeaError(:,:,MCNum)=[ObserveCar(1,:,MCNum)-RealState(1,:); ObserveCar(2,:,MCNum)-RealState(3,:)];
    
           

end
MeaErrorMean=mean(MeaError,3);
MeaErrorVar=std(MeaError,1,3);
EKFFilErrorMean=mean(EKFFilError,3);
EKFFilErrorVar=std(EKFFilError,1,3);
NPFFilErrorMean=mean(NPFFilError,3);
NPFFilErrorVar=std(NPFFilError,1,3);
time=(3:PtAcc)*T;
figure(1),
subplot(2,2,1);
plot(time,EKFFilErrorMean(1,3:PtAcc),'ro-',time,NPFFilErrorMean(1,3:PtAcc),'kx:',...
    time,MeaErrorMean(1,3:PtAcc),'bo-');
grid on;
subplot(2,2,2);
plot(time,EKFFilErrorMean(2,3:PtAcc),'ro-',time,NPFFilErrorMean(2,3:PtAcc),'kx:');
grid on;
subplot(2,2,3);
plot(time,EKFFilErrorMean(3,3:PtAcc),'ro-',time,NPFFilErrorMean(3,3:PtAcc),'kx:',...
    time,MeaErrorMean(2,3:PtAcc),'bo-');
grid on;
subplot(2,2,4);
plot(time,EKFFilErrorMean(4,3:PtAcc),'ro-',time,NPFFilErrorMean(4,3:PtAcc),'kx:');
grid on;
figure(2),
subplot(2,2,1);
plot(time,EKFFilErrorVar(1,3:PtAcc),'ro-',time,NPFFilErrorVar(1,3:PtAcc),'kx:',time,MeaErrorVar(1,3:PtAcc),'bo-');
grid on;
subplot(2,2,2);
plot(time,EKFFilErrorVar(2,3:PtAcc),'ro-',time,NPFFilErrorVar(2,3:PtAcc),'kx:');
grid on;
subplot(2,2,3);
plot(time,EKFFilErrorVar(3,3:PtAcc),'ro-',time,NPFFilErrorVar(3,3:PtAcc),'kx:',time,MeaErrorVar(2,3:PtAcc),'bo-');
grid on;
subplot(2,2,4);
plot(time,EKFFilErrorVar(4,3:PtAcc),'ro-',time,NPFFilErrorVar(4,3:PtAcc),'kx:');
grid on;
toc,

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -