% EKF UKF CKF 的三个算法
clear;
x=1;% 初始状态
x_estimate = 0.5;%初始状态的估计
xpre=x_estimate ;
e_x_estimate = x_estimate; %EKF的初始估计
u_x_estimate = x_estimate; %UKF的初始估计
c_x_estimate = x_estimate; %CKF的初始估计
Q = 10 % 过程状态协方差
R = 0.01 % 测量噪声协方差
P =1000;%初始估计方差
n=1; %%%%% 系统的维数
m=2*n; %%%%% CKF容积点数
w=1/m; %%%%%% 权值
kesi=[1,-1];%%%%% CKF容积点
e_P = P; %EKF方差
u_P = P;%UKF方差
Ppost=P;%CKF方差
tf = 100; % 模拟长度
x_array = [x];%真实值数组
e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组
u_x_estimate_array = [u_x_estimate];%UKF最优估计值数组
c_x_estimate_array = [c_x_estimate];%CKF最优估计值数组
u_k = 1; %微调参数
u_symmetry_number = 4; % 对称的点的个数
u_total_number = 2 * u_symmetry_number + 1; %总的采样点的个数
linear = 0.5;
close all;
for k = 1 : tf
% 模拟系统
% x = linear * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) %状态值
% x_array = [x_array,x];
% y = (x^2 / 20) + sqrt(R) * randn; %观测值
% z=y;
% x=x+ sqrt(Q) * randn;
x = 0.2*x+0.01*x^2+8*cos(1.2*(k+1)) +sqrt(10)*randn %状态值
x_array = [x_array,x];
y = x^2 + 0.1*randn; %观测值
z=y;
%扩展卡尔曼滤波器
%进行估计 第一阶段的估计
e_x_estimate_1 = 0.2 * e_x_estimate + 0.01* e_x_estimate^2 + 8 * cos(1.2*(k+1));
e_y_estimate = (e_x_estimate_1)^2;
%相关矩阵
e_A = 0.2 + 0.01* e_x_estimate;%传递矩阵
e_H = 2*e_x_estimate_1; %观测矩阵
%估计的误差
e_p_estimate = e_A * e_P * e_A' + Q;
%扩展卡尔曼增益
e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R);
%进行估计值的更新 第二阶段
e_x_estimate_2 = e_x_estimate_1 + e_K * (y - e_y_estimate);
%更新后的估计值的误差
e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate;
%进入下一次迭代的参数变化
e_P = e_p_estimate_update;
e_x_estimate = e_x_estimate_2;
%%%%%%无味卡尔曼滤波器UKF
%采样点的选取 存在x(i)
u_x_par = u_x_estimate;
for i = 2 : (u_symmetry_number+1)
u_x_par(i) = u_x_estimate + sqrt((u_symmetry_number+u_k) * u_P);
end
for i = (u_symmetry_number+2) : u_total_number
u_x_par(i) = u_x_estimate - sqrt((u_symmetry_number+u_k) * u_P);
end
%计算权值
u_w_1 = u_k/(u_symmetry_number+u_k);
u_w_N1 = 1/(2 * (u_symmetry_number+u_k));
%把这些粒子通过传递方程 得到下一个状态
for i = 1: u_total_number
u_x_par(i) = 0.2 * u_x_par(i) + 0.01* u_x_par(i)^2 + 8 * cos(1.2*(k+1));
end
%传递后的均值和方差
u_x_next = u_w_1 * u_x_par(1);
for i = 2 : u_total_number
u_x_next = u_x_next + u_w_N1 * u_x_par(i);
end
u_p_next = Q + u_w_1 * (u_x_par(1)-u_x_next) * (u_x_par(1)-u_x_next)';
for i = 2 : u_total_number
u_p_next = u_p_next + u_w_N1 * (u_x_par(i)-u_x_next) * (u_x_par(i)-u_x_next)';
end
for i = 1 :u_total_number
u_y_2obser(i,:) = u_x_par(i);
end
%通过观测方程 得到一系列的粒子
for i = 1: u_total_number
u_y_2obser(i) = u_y_2obser(i)^2;
end
%通过观测方程后的均值 y_obse
u_y_obse = u_w_1 * u_y_2obser(1);
for i = 2 : u_total_number
u_y_obse = u_y_obse + u_w_N1 * u_y_2obser(i);
end
%Pzz测量方差矩阵
u_pzz = R + u_w_1 * (u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)';
for i = 2 : u_total_number
u_pzz = u_pzz + u_w_N1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)';
end
%Pxz状态向量与测量值的协方差矩阵
u_pxz = u_w_1 * (u_x_par(1) - u_x_next)* (u_y_2obser(1)-u_y_obse)';
for i = 2 : u_total_number
u_pxz = u_pxz + u_w_N1 * (u_x_par(i) - u_x_next) * (u_y_2obser(i)- u_y_obse)';
end
%卡尔曼增益
u_K = u_pxz/u_pzz;
%估计量的更新
u_x_next_optimal = u_x_next + u_K * (y - u_y_obse);%第一步的估计值 + 修正值;
u_x_estimate = u_x_next_optimal;
%方差的更新
u_p_next_update = u_p_next - u_K * u_pzz * u_K';
u_P = u_p_next_update;
%%%%%——CKF滤波——%%%%%%%
%%%%%时间更新
%%%%%(1)求协方差矩阵平方根
Spost=sqrt(Ppost);
%%%%%(2)计算求容积点
rjpoint(1)=Spost*kesi(1)+xpre;
rjpoint(2)=Spost*kesi(2)+xpre;
%%%%%(3)传播求容积点
Xminus(1)=0.2*rjpoint(1)+0.01*rjpoint(1)^2+8*cos(1.2*(k+1)); %%%%容积点经过非线性函数后的值
Xminus(2)=0.2*rjpoint(2)+0.01*rjpoint(2)^2+8*cos(1.2*(k+1));
%%%%(4)状态预测
xminus=0.5*Xminus(1)+0.5*Xminus(2);
%%%%(5)状态预测协方差阵
Pminus=0.5*Xminus(1)^2+0.5*Xminus(2)^2-xminus*xminus'+Q;
%%%%观测更新
%%%%%(1)矩阵分解
Sminus=sqrt(Pminus-Q);
%%%%%(2)计算求容积点
rjpoint1(1)=Sminus*kesi(1)+xminus;
rjpoint1(2)=Sminus*kesi(2)+xminus;
%%%%%(3)传播求容积点
Z(1)=rjpoint1(1)^2;
Z(2)=rjpoint1(2)^2;
%%%%%%%(4)观测预测
zhat=0.5*Z(1)+0.5*Z(2);
%%%%(5)观测预测协方差阵
Pzminus=0.5*(Z(1)-zhat)^2+0.5*(Z(2)-zhat)^2+R;
%%%%(6)互协方差阵
Pxzminus=0.5*(rjpoint1(1)-xminus)*(Z(1)-zhat)+0.5*(rjpoint1(2)-xminus)*(Z(2)-zhat);
%%%%(7)计算卡尔曼增益
K=Pxzminus*inv(Pzminus);
%%%%(8)状态更新
xpre=xminus+K*(z-zhat);
%%%%(9)状态协方差矩阵更新
Ppost=Pminus-K*Pzminus*K';
c_x_estimate=xpre;
%进行画图程序
e_x_estimate_array = [e_x_estimate_array,e_x_estimate];
u_x_estimate_array = [u_x_estimate_array,u_x_estimate];
c_x_estimate_array = [c_x_estimate_array,c_x_estimate];
e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k));
u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k));
c_error(k,:)=abs(x_array(k)-c_x_estimate_array(k));
end
t = 0 : tf;
figure;
plot(t,x_array,'k-.',t,e_x_estimate_array,'r-',t,u_x_estimate_array,'b:',t,c_x_estimate_array,'c-.');
set(gca,'FontSize',10);
legend('真实值','EKF估计值','UKF估计值','CKF估计值');
xlabel('样本数目n')
ylabel('信号幅度')
%root mean square 平均值的平方根
e_xrms = sqrt((norm(x_array-e_x_estimate_array)^2)/tf);
disp(['EKF估计误差均方值=',num2str(e_xrms)]);
u_xrms = sqrt((norm(x_array-u_x_estimate_array)^2)/tf);
disp(['UKF估计误差均方值=',num2str(u_xrms)]);
c_xrms = sqrt((norm(x_array-c_x_estimate_array)^2)/tf);
disp(['CKF估计误差均方值=',num2str(c_xrms)]);
t = 1 : tf;
figure;
h=plot(t,e_error,'r-',t,u_error,'b:',t,c_error,'-c');
set(gca,'FontSize',10);
legend('EKF估计值误差','UKF估计值误差','CKF估计值误差');
xlabel('样本数目n')
ylabel('估计误差')
%toc;
评论10