📄 npfmain.asv
字号:
%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');
PtAcc=60;
MCAcc=10; %设定蒙特卡罗仿真次数
Ns=1000; %设定本程序中粒子滤波的粒子总数
%状态方程和观测方程中各矩阵和参数的定义
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;%0.2*pi/180;
SigmaBeta=5000;
EstBeta=Beta+normrnd(0,SigmaBeta);
R=[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];%0.001*SigmaBeta^2*T];
%状态方程和观测方程中各矩阵和参数定义结束
%各存储矩阵的初始化
EKFFilState=zeros(5,PtAcc);
MeaError=zeros(2,PtAcc,MCAcc);
MeaErrorMean=zeros(2,PtAcc);
MeaErrorVar=zeros(2,PtAcc);
EKFFilError=zeros(5,PtAcc,MCAcc);
EKFFilErrorMean=zeros(5,PtAcc);
EKFFilErrorVar=zeros(5,PtAcc);
FilErrMat=zeros(5,5,PtAcc);
Pw=zeros(Ns,PtAcc);
Particles=zeros(5,Ns,PtAcc);
ParticlesMCMC=zeros(5,Ns,PtAcc);
ParVar=zeros(5,5,Ns);
ParMean=zeros(5,Ns);
Probabilty1=zeros(Ns,1);
Order=zeros(Ns,1);
for i=1:Ns
Order(i)=i;
end
for MCNum=1:MCAcc
Observe(1,:,MCNum)=sqrt(RealState(1,:).*RealState(1,:)+RealState(3,:).*RealState(3,:))+Glint(0.2,SigmaR,1,PtAcc);
Observe(2,:,MCNum)=atan2(RealState(3,:),RealState(1,:))+Glint(0.2,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(0.2,SigmaR,1,Ns);%8*SigmaR*(rand(1,Ns)-0.5);%
ParA2=Observe(2,2,MCNum)+Glint(0.2,SigmaA,1,Ns);%8*SigmaA*(rand(1,Ns)-0.5);%
ParR1=Observe(1,1,MCNum)+Glint(0.2,SigmaR,1,Ns);%8*SigmaR*(rand(1,Ns)-0.5);%
ParA1=Observe(2,1,MCNum)+Glint(0.2,SigmaA,1,Ns);%8*SigmaA*(rand(1,Ns)-0.5);%
Particles(1,:,2)=ParR2.*cos(ParA2);%RealState(1,2)+normrnd(0,1000,1,Ns);
Particles(2,:,2)=(ParR2.*cos(ParA2)-ParR1.*cos(ParA1))/T;%RealState(2,2)+normrnd(0,50,1,Ns);
Particles(3,:,2)=ParR2.*sin(ParA2);%RealState(3,2)+normrnd(0,1000,1,Ns);
Particles(4,:,2)=(ParR2.*sin(ParA2)-ParR1.*sin(ParA1))/T;%RealState(4,2)+normrnd(0,50,1,Ns);
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); %更新粒子
Temp=hnonl(Particles(:,ParNum,PtNum));
Temp1=Observe(:,PtNum,MCNum)-Temp;
Pw(ParNum,PtNum)=(0.8/sqrt(2*pi*R(1,1))*exp(-0.5*Temp1(1)^2/R(1,1))+0.2/(6*SigmaR)*exp(-abs(Temp1(1))/(3*SigmaR)))*...
(0.8/sqrt(2*pi*R(2,2))*exp(-0.5*Temp1(2)^2/R(2,2))+0.2/(6*SigmaR)*exp(-abs(Temp1(1))/(3*SigmaR)));
Pw(ParNum,PtNum)=0.8/(2*pi*sqrt(det(R)))*exp(-0.5*(Observe(:,PtNum,MCNum)-Temp)'*inv(R)*(Observe(:,PtNum,MCNum)-Temp))+...
0.2/(6*SigmaR)/(6*SigmaA)*exp(-abs(Observe(1,PtNum,MCNum)-Temp(1))/(3*SigmaR)-abs(Observe(2,PtNum,MCNum)-Temp(2))/(3*SigmaA));
end
Pw(:,PtNum)=Pw(:,PtNum)/sum(Pw(:,PtNum));
Neff(PtNum)=1/sum(Pw(:,PtNum).*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 + -