CKF MCSCKF UKF EKF滤波性能对比
在非线性滤波中,比较了CKF MCSCKF UKF EKF 几种非线性滤波的性能 用MATLAB进行仿真.八维非线性滤波中,CKF,MCSCKF 比较稳定.EKF UKF 表现不好.
MATLAB代码如下
%%%%%%%%%%%%%%%%ZG%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 平方根法 容积Kalman滤波 MCSCKF
% 状态方程:x(:,k+1) = F * x(:,k) +[sqrt(Q) * randn];
% 观测方程:z(k+1) = atan(0.1 * x(1,k+1)) + sqrt(R) * randn;
clc; clear all;close all;
%系统初始化状态变量。误差协方差,过程噪声,测量噪声
QQ=0.2*eye(8);
RR=0.5*eye(8);
%R_ku=[1,0;0,1];
X0=[1;2;3;4;5;6;7;8];
n=8; %维数
P0=2*eye(8);
N=100;
W=sqrt(QQ)*randn(n,N);
V=sqrt(RR)*randn(n,N);
%V=0.8*(10+sqrt(RR)*randn(n,N))+0.2*(1+sqrt(RR)*randn(n,N));
X=zeros(n,N);
Y=zeros(n,N);
for k=1:N
if k==1
X(:,1)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),X0)+W(k);
else
X(:,k)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),X(:,k-1))+W(k);
end
end
for k=1:N
Y(:,k)=arrayfun(@(X)(X^2)/20,X(:,k))+V(k);
end
w=1/(2*n); %每个容积点对应的权值
m=2*n;%容积点个数
kesi=sqrt(m/2)*[eye(n),-eye(n)];%对应的容积点集
%模拟观测阶段
PP=P0;
XX=X(:,1);
S0=chol(PP,'lower');
for k = 2 : N
%状态的更新:
%基于状态估计的容积点预测:
%%%%%(1)求协方差矩阵平方根
S=S0;
%[Q,R]=qr(QQ);
SQ=chol(QQ,'lower');
SR=chol(RR,'lower');
%%%%%(2)计算求容积点
rjpoint=zeros(n,m);
for t=1:m
rjpoint(:,t)=S*kesi(:,t)+XX;
end
%%%%%(3)传播求容积点
x_pre=zeros(n,m);
for t=1:m
x_pre(:,t)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),rjpoint(:,t))
end
%%%%(4)状态预测
X_pre=zeros(n,1);
for t=1:m
X_pre=X_pre+w*x_pre(:,t) %w=1/2n
end
XXX_pre=zeros(n,m);
AB=ones(n,m);
%XXX_pre=[X_pre,X_pre,X_pre,X_pre,X_pre,X_pre,X_pre,X_pre];%%%%mcsckf
for t=1:m;
XXX_pre(:,t)=X_pre.*AB(:,t)
end
xx_pre=zeros(n,m);
xx_pre=sqrt(w)*(x_pre-XXX_pre);
%%%%(5)状态预测协方差阵
P_pre=zeros(n,n);
for t=1:m
P_pre=P_pre+w*(x_pre(:,t)*x_pre(:,t)')
end
%%%%观测更新
%%%%%(1)矩阵分解
[Q,R]=qr([xx_pre,SQ]',0);
S_pre=R';
%%%%%(2)计算求容积点
%rjpoint1=zeros(2,m);
for t=1:m
rjpoint1(:,t)=S_pre*kesi(:,t)+X_pre;
end
%%%%%(3)传播求容积点
y_pre=zeros(n,m);
for t=1:m
y_pre(:,t)=arrayfun(@(X)(X^2)/20,rjpoint1(:,t));
end
%%%%%%%(4)观测预测
Y_pre=zeros(n,1);
for t=1:m
Y_pre=Y_pre+w*y_pre(:,t)
end
%%%%(5)观测预测协方差阵
YYY_pre=zeros(n,m);
AB=ones(n,m);
for t=1:m
YYY_pre(:,t)=Y_pre.*AB(:,t) %%%%mcsckf
end
yy_pre=zeros(n,m);
yy_pre=sqrt(w)*(y_pre-YYY_pre);
[Q,R]=qr([yy_pre,SR]',0);
Syy_pre=R';
xxx_pre=zeros(n,m);
xxx_pre=sqrt(w)*(rjpoint1-XXX_pre);
Sxy_pre=xxx_pre*yy_pre';
%%%%(7)计算卡尔曼增益
K=(Sxy_pre*inv(Syy_pre'))*inv(Syy_pre);
%%%%(8)状态更新
X_est=X_pre+K*(Y(:,k)-Y_pre);
%%%%(9)状态协方差矩阵更新
[Q,R]=qr([xxx_pre-K*yy_pre,K*SR]',0);
S0=R';
XX=X_est;
PP=S0*S0';
P_est1(:,:,k)=PP;
X_est1(:,k)=X_est;
X_pre1(:,k)=X_pre;
Y_pre1(:,k)=Y_pre;
P_est1(:,:,k)=S0*S0';
K1(:,:,k)=K;
end
X_est1(:,1)=X(:,1);
P_est1(:,:,1)=P0;
for k=1:N
Perro1(1,k)=P_est1(5,5,k);%估计误差协方差的误差,准确值
Perro1(2,k)=P_est1(7,7,k);
end
%%%%%%%%%%%%%%%%%%%%%%%%CKF%%%%%%%%%%%%
PP=P0;
XX=X(:,1);
for k = 2 : N
%Cubature卡尔曼滤波器
%(2)计算容积点
%%%%%(1)求协方差矩阵平方根
S=chol(PP,'lower');
%%%%%(2)计算求容积点
rjpoint=zeros(n,m);
for t=1:m
rjpoint(:,t)=S*kesi(:,t)+XX;
end
%%%%%(3)传播求容积点
x_pre=zeros(n,m);
for t=1:m
x_pre(:,t)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),rjpoint(:,t))
end
%%%%(4)状态预测
X_pre=zeros(n,1);
for t=1:m
X_pre=X_pre+w*x_pre(:,t) %w=1/2n
end
%%%%(5)状态预测协方差阵
P_pre=zeros(n,n);
for t=1:m
P_pre=P_pre+w*(x_pre(:,t)*x_pre(:,t)')
end
P_pre=P_pre-X_pre*X_pre'+QQ;
%%%%观测更新
%%%%%(1)矩阵分解
S_pre=chol(P_pre,'lower');
%%%%%(2)计算求容积点
rjpoint1=zeros(n,m);
for t=1:m
rjpoint1(:,t)=S_pre*kesi(:,t)+X_pre;
end
%%%%%(3)传播求容积点
y_pre=zeros(n,m);
for t=1:m
y_pre(:,t)=arrayfun(@(X)(X^2)/20,rjpoint1(:,t));
end
%%%%%%%(4)观测预测
Y_pre=zeros(n,1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=1:m
Y_pre=Y_pre+w*y_pre(:,t)
end
%%%%(5)观测预测协方差阵
Pyy=zeros(n,n);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=1:m
Pyy=Pyy+w*(y_pre(:,t)*y_pre(:,t)')
end
Pyy=Pyy-Y_pre*Y_pre'+RR;
%%%%(6)互协方差阵
Pxy=zeros(n,n);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for t=1:m
Pxy=Pxy+w*(rjpoint1(:,t)*y_pre(:,t)')
end
Pxy=Pxy-X_pre*Y_pre';
%%%%(7)计算卡尔曼增益
K=Pxy*inv(Pyy);
%%%%(8)状态更新
X_est=X_pre+K*(Y(:,k)-Y_pre);
%%%%(9)状态协方差矩阵更新
PP=P_pre-K*Pyy*K';
XXXXX=X_est;
PPP=PP;
P_est2(:,:,k)=PPP;
X_est2(:,k)=XXXXX;
X_pre2(:,k)=X_pre;
Y_pre2(:,k)=Y_pre;
Pxy2(:,:,k)=Pxy;
Pyy2(:,:,k)=Pyy;
K2(:,:,k)=K;
end
X_est2(:,1)=X(:,1);
P_est2(:,:,1)=P0;
for k=1:N
Perro2(1,k)=P_est2(5,5,k);%估计误差协方差的误差,准确值
Perro2(2,k)=P_est2(7,7,k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%EKF%%%%%%%%%%%%%%%%%%%%%%%%%%
I=eye(n);
for k=2:N
X_est=XX;
P_est=PP;
X_pre=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),X(:,k-1));%状态预测
Y_pre=arrayfun(@(X)(X^2)/20,X_pre);
F_1=arrayfun(@(X)0.5+(2.5-2.5*X^2)/(1+X^2)^2,X_pre);
AB=ones(n);
F=zeros(n);
for t=1:n
F(:,t)=F_1.*AB(:,t);
end
F=F'
%F=[F_1';F_1';F_1';F_1';F_1';F_1';F_1';F_1'];
H_1=arrayfun(@(X)X/10,X_pre);
AB=ones(n);
H=zeros(n);
for t=1:n
H(:,t)=H_1.*AB(:,t);
end
H=H'
%H=[H_1';H_1';H_1';H_1';H_1';H_1';H_1';H_1'];
P_pre=F*P_est*F'+QQ;
Pxy=P_pre*H';
Pyy=H*P_pre*H'+RR;
K=Pxy*inv(Pyy);
X_est=X_pre+K*(Y(:,k)-Y_pre);
P_est=(I-K*H)*P_pre;
XX=X_est;
PP=P_est;
XXXXXX=XX;
PPPPPP=PP;
X_est3(:,k)=XXXXXX;
X_pre3(:,k)=X_pre;
Y_pre3(:,k)=Y_pre;
P_est3(:,:,k)=PPPPPP;
K1(:,:,k)=K;
end
X_est3(:,1)=X(:,1);
P_est3(:,:,1)=P0;
for k=1:N
Perro3(1,k)=P_est3(5,5,k);%估计误差协方差的误差,准确值
Perro3(2,k)=P_est3(7,7,k);
end
a=0.01;
k_=0;
b=2;
%n=2;%%维数
ramda=a*a*(n+k_)-n; %这个不对,存疑
%ramda=3-n;
%ramda=0.01
for j=1:2*n+1
Wm(j)=1/(2*(n+ramda));
Wc(j)=1/(2*(n+ramda));
end
Wm(1)=ramda/(n+ramda);
Wc(1)=ramda/(n+ramda)+1-a^2+b;
%%%%%%% UKF %%%%%%%%%%%%%%%%
XX=X0;%初始化X的值
PP=P0;%初始化P的值
X_est=zeros(n,1);
for t=2:N
X_est=XX;
P_est=PP;
%X的sigma点集
cho=(chol(P_est.*(n+ramda)));
for k=1 :n
xgamaP1(:,k)=X_est+cho(:,k);
xgamaP2(:,k)=X_est-cho(:,k);
end
Xsigama=zeros(n,2*n+1);
Xsigama=[X_est,xgamaP1,xgamaP2];
%X的sigma预测值
Xsigama_pre=zeros(n,2*n+1);
for k=1:2*n+1
Xsigama_pre(:,k)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*t),Xsigama(:,k))
end
%Xsigama_pre=1./(Xsigama.^2+1);
X_pre=zeros(n,1);
for k=1:2*n+1
X_pre=X_pre+Wm(k).*Xsigama_pre(:,k);
end
%P的预测值
P_pre=zeros(n,n);
for k=1:2*n+1
P_pre=P_pre+Wc(k).*(Xsigama_pre(:,k)-X_pre)*(Xsigama_pre(:,k)-X_pre)';
end
P_pre=P_pre+QQ;
%%再次UT变换
chor=(chol(P_pre*(n+ramda)));
for k=1 :n
xutgamaP1(:,k)=X_pre+chor(:,k);
xutgamaP2(:,k)=X_pre-chor(:,k);
end
Ysigama=[X_pre,xutgamaP1,xutgamaP2];
%Y的sigma预测值
Ysigama_pre=zeros(n,2*n+1);
for k=1:2*n+1
Ysigama_pre(:,k)=arrayfun(@(X)(X^2)/20,Ysigama(:,k))
end
%Ysigama_pre=1./(Ysigama.^2+1);
%Y的预测值
Y_pre=zeros(n,1);
for k=1:2*n+1
Y_pre=Y_pre+Wm(k)*Ysigama_pre(:,k);
end
Pyy=zeros(n,n)
for k=1:2*n+1
Pyy=Pyy+Wc(k)*((Ysigama_pre(:,k)-Y_pre)*(Ysigama_pre(:,k)-Y_pre)');
end
Pyy=Pyy+RR;
Pxy=zeros(n,n)
for k=1:2*n+1
Pxy=Pxy+Wc(k)*((Xsigama_pre(:,k)-X_pre)*(Ysigama_pre(:,k)-Y_pre)');
end
K=Pxy*inv(Pyy); %增益系数
X_est=X_pre+K*(Y(:,t)-Y_pre);%X的估计值
P_est=P_pre-K*Pyy*K';%P的估计值
PP=P_est; %程序数据更新
XX=X_est;
XXXXXX=XX;
PPPPPP=PP;
%程序数据更新
%%%%以下为数据记录
X_est4(:,t)=XXXXXX;
X_pre4(:,t)=X_pre;
Y_pre4(:,t)=Y_pre;
P_est4(:,:,t)=PPPPPP;
Pxy4(:,:,t)=Pxy;
Pyy4(:,:,t)=Pyy;
K4(:,:,t)=K;
end
X_est4(:,1)=X(:,1);
P_est4(:,:,1)=P0;
for k=1:N
Perro4(1,k)=P_est4(5,5,k);%估计误差协方差的误差,准确值
Perro4(2,k)=P_est4(7,7,k);
end
k = 1 : N;
figure(1);
plot(k,X(5,k),'g-d',k,X_est1(5,k),'r-*',k,X_est2(5,k),'b-^',k,X_est3(5,k),'m-x',k,X_est4(5,k),'c-p');
legend('真实值','MCSCKF估计值','CKF估计值','EKF估计值','UKF估计值');
figure(2);
plot(k,X(8,k),'g-d',k,X_est1(8,k),'r-*',k,X_est2(8,k),'b-*',k,X_est3(8,k),'m-x',k,X_est4(8,k),'c-p');
legend('真实值','MCSCKF估计值','CKF估计值','EKF估计值','UKF估计值');
figure(3)
k=1:N;
plot(k,Perro1(1,k),'g-d',k,Perro2(1,k),'b-*',k,Perro3(1,k),'m-x',k,Perro4(1,k),'c-p');
xlabel('时间'),ylabel('误差');
legend ('MCSCKF','CKF','EKF','UKF');
title('估计误差协方差1')
figure(4)
k=1:N;
plot(k,Perro1(2,k),'g-d',k,Perro2(2,k),'b-*',k,Perro3(2,k),'m-x',k,Perro4(2,k),'c-p');
xlabel('时间'),ylabel('误差');
legend ('MCSCKF','CKF','EKF','UKF');
title('估计误差协方差2')```
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)