CKF MCSCKF UKF EKF滤波性能对比

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')```