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:Nif k==1X(:,1)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),X0)+W(k);elseX(:,k)=arrayfun(@(X)0.5*X+2.5*X/(1+X^2)+2*cos(1.2*k),X(:,k-1))+W(k);end
endfor k=1:NY(:,k)=arrayfun(@(X)(X^2)/20,X(:,k))+V(k);endw=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:mrjpoint(:,t)=S*kesi(:,t)+XX;end%%%%%（3）传播求容积点x_pre=zeros(n,m);for t=1:mx_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:mX_pre=X_pre+w*x_pre(:,t)    %w=1/2nendXXX_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];%%%%mcsckffor t=1:m;XXX_pre(:,t)=X_pre.*AB(:,t)endxx_pre=zeros(n,m);xx_pre=sqrt(w)*(x_pre-XXX_pre);%%%%(5)状态预测协方差阵P_pre=zeros(n,n);for t=1:mP_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:mrjpoint1(:,t)=S_pre*kesi(:,t)+X_pre;end%%%%%（3）传播求容积点y_pre=zeros(n,m);for t=1:my_pre(:,t)=arrayfun(@(X)(X^2)/20,rjpoint1(:,t));end%%%%%%%（4）观测预测Y_pre=zeros(n,1);for t=1:mY_pre=Y_pre+w*y_pre(:,t)end%%%%(5)观测预测协方差阵YYY_pre=zeros(n,m);AB=ones(n,m);for t=1:mYYY_pre(:,t)=Y_pre.*AB(:,t) %%%%mcsckfendyy_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:NPerro1(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:mrjpoint(:,t)=S*kesi(:,t)+XX;end%%%%%（3）传播求容积点x_pre=zeros(n,m);for t=1:mx_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:mX_pre=X_pre+w*x_pre(:,t)    %w=1/2nend%%%%(5)状态预测协方差阵P_pre=zeros(n,n);for t=1:mP_pre=P_pre+w*(x_pre(:,t)*x_pre(:,t)')endP_pre=P_pre-X_pre*X_pre'+QQ;%%%%观测更新%%%%%（1）矩阵分解S_pre=chol(P_pre,'lower');%%%%%（2）计算求容积点rjpoint1=zeros(n,m);for t=1:mrjpoint1(:,t)=S_pre*kesi(:,t)+X_pre;end%%%%%（3）传播求容积点y_pre=zeros(n,m);for t=1:my_pre(:,t)=arrayfun(@(X)(X^2)/20,rjpoint1(:,t));end%%%%%%%（4）观测预测Y_pre=zeros(n,1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for t=1:mY_pre=Y_pre+w*y_pre(:,t)end%%%%(5)观测预测协方差阵Pyy=zeros(n,n);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for t=1:mPyy=Pyy+w*(y_pre(:,t)*y_pre(:,t)')endPyy=Pyy-Y_pre*Y_pre'+RR;%%%%(6)互协方差阵Pxy=zeros(n,n);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for t=1:mPxy=Pxy+w*(rjpoint1(:,t)*y_pre(:,t)')endPxy=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:NPerro2(1,k)=P_est2(5,5,k);%估计误差协方差的误差,准确值Perro2(2,k)=P_est2(7,7,k);end
%%%%%%%%%%%%%%%%%%%%%%%%%EKF%%%%%%%%%%%%%%%%%%%%%%%%%%
I=eye(n);
for k=2:NX_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:nF(:,t)=F_1.*AB(:,t);endF=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:nH(:,t)=H_1.*AB(:,t);endH=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:NPerro3(1,k)=P_est3(5,5,k);%估计误差协方差的误差,准确值Perro3(2,k)=P_est3(7,7,k);
enda=0.01;
k_=0;
b=2;
%n=2;%%维数
ramda=a*a*(n+k_)-n;  %这个不对,存疑
%ramda=3-n;
%ramda=0.01for j=1:2*n+1Wm(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:NX_est=XX;P_est=PP;%X的sigma点集cho=(chol(P_est.*(n+ramda)));for k=1 :nxgamaP1(:,k)=X_est+cho(:,k);xgamaP2(:,k)=X_est-cho(:,k);endXsigama=zeros(n,2*n+1);Xsigama=[X_est,xgamaP1,xgamaP2];%X的sigma预测值Xsigama_pre=zeros(n,2*n+1);for k=1:2*n+1Xsigama_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+1X_pre=X_pre+Wm(k).*Xsigama_pre(:,k);end%P的预测值P_pre=zeros(n,n);for k=1:2*n+1P_pre=P_pre+Wc(k).*(Xsigama_pre(:,k)-X_pre)*(Xsigama_pre(:,k)-X_pre)';endP_pre=P_pre+QQ;%%再次UT变换chor=(chol(P_pre*(n+ramda)));for k=1 :nxutgamaP1(:,k)=X_pre+chor(:,k);xutgamaP2(:,k)=X_pre-chor(:,k);endYsigama=[X_pre,xutgamaP1,xutgamaP2];%Y的sigma预测值Ysigama_pre=zeros(n,2*n+1);for k=1:2*n+1Ysigama_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+1Y_pre=Y_pre+Wm(k)*Ysigama_pre(:,k);endPyy=zeros(n,n)for k=1:2*n+1Pyy=Pyy+Wc(k)*((Ysigama_pre(:,k)-Y_pre)*(Ysigama_pre(:,k)-Y_pre)');endPyy=Pyy+RR;Pxy=zeros(n,n)for k=1:2*n+1Pxy=Pxy+Wc(k)*((Xsigama_pre(:,k)-X_pre)*(Ysigama_pre(:,k)-Y_pre)');endK=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:NPerro4(1,k)=P_est4(5,5,k);%估计误差协方差的误差,准确值Perro4(2,k)=P_est4(7,7,k);
endk = 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')```

CKF MCSCKF UKF EKF滤波性能对比相关推荐

CDKF、UKF和EKF滤波算法
转载自:https://mp.weixin.qq.com/s/umv72zAB-i3luzyIvXpvYw CDKF.UKF和EKF滤波算法凌拓智能TBUS TBUS社区今天之前一直和大家探讨在 ...
SLAM导航机器人零基础实战系列：（三）感知与大脑——5.机器人大脑嵌入式主板性能对比...
SLAM导航机器人零基础实战系列:(三)感知与大脑--5.机器人大脑嵌入式主板性能对比摘要在我的想象中机器人首先应该能自由的走来走去,然后应该能流利的与主人对话.朝着这个理想,我准备设计一个能自由 ...
水下机器人二维变速圆周运动的SBL定位EKF滤波仿真分析
水下机器人在水下想要获得定位信息目前主要是基于声学方式的长基线(LBL).短基线(SBL).超短基线(USBL)获得局部定位数据,还有通过地磁场或者重力场匹配导航的方式,这类方式目前仅限于军事领域.当 ...
iPhone6、iPad Pro惯性传感器性能对比
iPhone6.iPad Pro惯性传感器性能对比引言 1.标定 2.结果 4.对比分析 4.Allan Variance分析 5.结果 6.TODO 引言厉害!原文是github.io制作的博客 ...
Java常用消息队列原理介绍及性能对比
消息队列使用场景为什么会需要消息队列(MQ)? 解耦在项目启动之初来预测将来项目会碰到什么需求,是极其困难的.消息系统在处理过程中间插入了一个隐含的.基于数据的接口层,两边的处理过程都要实现这一 ...
golang连接postgresql too many client_MySQL和PostgreSQL压测性能对比
阅读使人充实,讨论使人敏捷,写作使人精确. >>> 压测业务场景文章属于互联网社区动态类场景核心功能压测案例.至于题目涉及的MySQL和PostgreSQL之间的关系,主要为业务选型 ...
php下curl与file_get_contents性能对比
为什么80%的码农都做不了架构师?>>> 上一篇讲了 <php使用curl替代file_get_contents>, 后续贴出了curl和file_get_cont ...
p40与p100训练性能对比
深度学习训练,选择P100就对了原文:https://yq.aliyun.com/articles/238764 摘要: 本文使用NVCaffe.MXNet.TensorFlow三个主流开源深度学习 ...
php vs lua,解析LUA与PHP在WEB应用的性能对比
解析LUA与PHP在WEB应用的性能对比是本文要介绍的内容,这几天用在WEB开发的LUA框架已经完成,框架中已包括数据库操作和模板操作的功能,能够很简单方便的应用在WEB开发上.在此时我对这个LUA框 ...

CKF MCSCKF UKF EKF滤波性能对比

CKF MCSCKF UKF EKF滤波性能对比

CKF MCSCKF UKF EKF滤波性能对比相关推荐

最新文章

热门文章