信息融合作业_UKF_一维

clear all;
close all;
clc

Q=0.01;
R=0.01;
X0=1;
P0=0.01;
N=100;
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);

%模型
for k=1:N
   if k==1
   X(1)=X0+1/X0+W(k);
   else
X(k)=(X(k-1)+1/X(k-1))+W(k);
   end
end
for k=1:N
   if k==1
   Y(1)=X0^2+V(k);
   else
Y(k)=X(k)*X(k)+V(k);
   end
end

a=0.01;
k_=0;
b=2;
n=1;%%维数
%ramda=a*a*(n+k_)-n;  这个不对,存疑
ramda=0.01;%翻阅资料,randa取0.01后X估计值跟上了

Wm(2)=0.5*(n+ramda);
Wm(3)=0.5*(n+ramda);
Wc(2)=Wm(2);
Wc(3)=Wm(3);
Wm(1)=ramda/(n+ramda);
Wc(1)=(ramda/(n+ramda))+(1-a*a+b);
    %%%%%%%%%%%%%%%%%%%%%%%
    %%%f(x)=x+1/x,h(x)=x*x,f'(x)=1-1/(x*x),h'(x)=2*x;

for k=1:N
    if k==1
%X的sigma点集     
xgamaP0(k)=X0;
xgamaP1(k)=X0+sqrt((n+ramda)*P0);
xgamaP2(k)=X0-sqrt((n+ramda)*P0);  
%X的sigma预测值
Xsigma_prea(k)=xgamaP0(k)+1/xgamaP0(k);
Xsigma_preb(k)=xgamaP1(k)+1/xgamaP1(k);
Xsigma_prec(k)=xgamaP2(k)+1/xgamaP2(k);%把sigma点带入f(x)=x+1/x
%X的预测值
X_pre(k)=Wm(1)*Xsigma_prea(k)+Wm(2)*Xsigma_preb(k)+Wm(3)*Xsigma_prec(k);
%P的预测值
P_pre(k)=Wc(1)*(X_pre(k)-Xsigma_prea(k))*(X_pre(k)-Xsigma_prea(k))'+Wc(2)*(X_pre(k)-Xsigma_preb(k))*(X_pre(k)-Xsigma_preb(k))'+Wc(3)*(X_pre(k)-Xsigma_prec(k))*(X_pre(k)-Xsigma_prec(k))'+Q;
%%再次UT变换
xutgamaP0(k)=X_pre(k);
xutgamaP1(k)=X_pre(k)+sqrt((n+ramda)*P_pre(k));
xutgamaP2(k)=X_pre(k)-sqrt((n+ramda)*P_pre(k));
%Y的sigma预测值
Ysigma_prea(k)=xutgamaP0(k)*xutgamaP0(k);
Ysigma_preb(k)=xutgamaP1(k)*xutgamaP1(k);
Ysigma_prec(k)=xutgamaP2(k)*xutgamaP2(k);%把sigma点带入h(x)=x^2
%Y的预测值
Y_pre(k)=Wm(1)*Ysigma_prea(k)+Wm(2)*Ysigma_preb(k)+Wm(3)*Ysigma_prec(k);
Pxy(k)=Wc(1)*(X_pre(k)-Xsigma_prea(k))*(Y_pre(k)-Ysigma_prea(k))'+Wc(2)*(X_pre(k)-Xsigma_preb(k))*(Y_pre(k)-Ysigma_preb(k))'+Wc(3)*(X_pre(k)-Xsigma_prec(k))*(Y_pre(k)-Ysigma_prec(k))';
Pyy(k)=Wc(1)*(Y_pre(k)-Ysigma_prea(k))*(Y_pre(k)-Ysigma_prea(k))'+Wc(2)*(Y_pre(k)-Ysigma_preb(k))*(Y_pre(k)-Ysigma_preb(k))'+Wc(3)*(Y_pre(k)-Ysigma_prec(k))*(Y_pre(k)-Ysigma_prec(k))'+R;

K(k)=Pxy(k)*inv(Pyy(k)); %增益系数
X_est(k)=X_pre(k)+K(k)*(Y(k)-Y_pre(k));%X的估计值
P_est(k)=P_pre(k)-K(k)*Pyy(k)*K(k)';%P的估计值

    else
 %X的UT变换,X的sigma点集    
xgamaP0(k)=X_est(k-1);  
xgamaP1(k)=X_est(k-1)+sqrt((n+ramda)*P_est(k-1));
xgamaP2(k)=X_est(k-1)-sqrt((n+ramda)*P_est(k-1));
%X 的sigma预测值
Xsigma_prea(k)=xgamaP0(k)+1/xgamaP0(k); 
Xsigma_preb(k)=xgamaP1(k)+1/xgamaP1(k);
Xsigma_prec(k)=xgamaP2(k)+1/xgamaP2(k);%把sigma点带入f(x)=x+1/x
%X的预测值
X_pre(k)=Wm(1)*Xsigma_prea(k)+Wm(2)*Xsigma_preb(k)+Wm(3)*Xsigma_prec(k);
%P的预测值
P_pre(k)=Wc(1)*(X_pre(k)-Xsigma_prea(k))*(X_pre(k)-Xsigma_prea(k))'+Wc(2)*(X_pre(k)-Xsigma_preb(k))*(X_pre(k)-Xsigma_preb(k))'+Wc(3)*(X_pre(k)-Xsigma_prec(k))*(X_pre(k)-Xsigma_prec(k))'+Q;

%%再次UT变换
xutgamaP0(k)=X_pre(k);
xutgamaP1(k)=X_pre(k)+sqrt((n+ramda)*P_pre(k));
xutgamaP2(k)=X_pre(k)-sqrt((n+ramda)*P_pre(k));
%Y 的sigma预测值
Ysigma_prea(k)=xutgamaP0(k)*xutgamaP0(k); %Y 的sigma预测值
Ysigma_preb(k)=xutgamaP1(k)*xutgamaP1(k);
Ysigma_prec(k)=xutgamaP2(k)*xutgamaP2(k);%把sigma点带入h(x)=x^2
%Y 的预测值
Y_pre(k)=Wm(1)*Ysigma_prea(k)+Wm(2)*Ysigma_preb(k)+Wm(3)*Ysigma_prec(k);
Pxy(k)=Wc(1)*(X_pre(k)-Xsigma_prea(k))*(Y_pre(k)-Ysigma_prea(k))'+Wc(2)*(X_pre(k)-Xsigma_preb(k))*(Y_pre(k)-Ysigma_preb(k))'+Wc(3)*(X_pre(k)-Xsigma_prec(k))*(Y_pre(k)-Ysigma_prec(k))';
Pyy(k)=Wc(1)*(Y_pre(k)-Ysigma_prea(k))*(Y_pre(k)-Ysigma_prea(k))'+Wc(2)*(Y_pre(k)-Ysigma_preb(k))*(Y_pre(k)-Ysigma_preb(k))'+Wc(3)*(Y_pre(k)-Ysigma_prec(k))*(Y_pre(k)-Ysigma_prec(k))'+R;
K(k)=Pxy(k)*inv(Pyy(k)); %增益系数
X_est(k)=X_pre(k)+K(k)*(Y(k)-Y_pre(k));%X估计值
P_est(k)=P_pre(k)-K(k)*Pyy(k)*K(k)';%P估计值
    end
end
%绘图分析:
figure(1)
k=1:N;
plot(k,X(k),'-o',k,X_est(k),'-x');
xlabel('时间'),ylabel('状态值');
legend ('X的状态值','X的估计值');
title('UKF算法X的状态值和估计值曲线');
for k = 1:N
    e(k)=X(k)-X_est(k);
end

figure(2)
k=1:N;
plot(k,e(k),'-');
xlabel('时间');
ylabel('误差');
title('UKF算法X的状态值与估计值的误差');

figure(3)
plot(k,P_est(k),'-');
xlabel('时间'),ylabel('误差');
title('UKF算法估计误差协方差曲线');



图三估计误差协方差有误.当ranmda取0时候,P_est无限变小.ramda=a*a*(n+k_)-n;公式在此不适用.