Unverified Commit 5f4d3096 authored by tyliu22's avatar tyliu22 Committed by GitHub

Add files via upload

parent c52e5130
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 分数阶卡尔曼滤波器仿真复现
% 论文: fractional order CDKF
% 目的:FCDKF与FEKF的性能比较
% 函数实验: D^{0.7} x_k = 3*sin(2*x_{k-1}) -x_{k-1} + w_k
% y_k = x_k + v_k
% 结果:
%
% 备注:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear
%仿真步长
N = 50;
h_con = sqrt(4);
q = 0.5; %系统噪声均值
r = 0.5; %测量噪声均值
Q = 0.81; %系统噪声方差矩阵
R = 0.81; %测量噪声方差矩阵
%GL定义下短记忆原理的长度
L = N+1;
%计算alpha阶次对应的GL定义系数 binomial coefficient
bino_fir = zeros(1,N); %微分阶次为0.7时GL定义下的系数
alpha = 0.7;
bino_fir(1,1) = 1;
for i = 2:1:N
bino_fir(1,i) = (1-(alpha+1)/(i-1))*bino_fir(1,i-1);
end
%系统矩阵设置
I = eye(1,1); %生成单位阵
%状态测量初始化
X_real = zeros(1,N); %真实状态
Z_meas = zeros(1,N); %实际观测值
%噪声
W_noise = sqrt(Q)*randn(1,N) + q; %系统噪声
V_noise = sqrt(R)*randn(1,N) + r; %测量噪声
x_0 = 0; %初始状态
X_real(1,1) = x_0; %真实状态初始值
Z_meas(1,1) = V_noise(1,1); %测量数据初始值
% 系统函数与测量函数
f=@(x)3*sin(2*x)-x;
h=@(x)x;
for k=2:1:N
%计算实际状态
diff_X_real = f(X_real(1,k-1)) + W_noise(1,k-1);
rema = 0;
for i = 2:1:k
rema = rema + bino_fir(1,i)*X_real(1,k+1-i);
end
X_real(1,k) = diff_X_real - rema;
%实际观测值
Z_meas(1,k) = h(X_real(1,k)) + V_noise(1,k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-------------------------分数阶卡尔曼滤波器性能测试------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X_esti = zeros(1,N); %状态最优估计值
P_xesti = zeros(1,N); %估计误差方差阵
P_pred_0 = 100; %初始预测方差阵
P_xesti(1,1) = P_pred_0; %初始估计方差阵
for k=2:1:N
%卡尔曼滤波
%状态预测:X_pre
diff_X_esti = f(X_esti(1,k-1));
%计算余项
rema = 0;
if k>L
for i = 2:1:L+1
rema = rema + bino_fir(1,i)*X_esti(1,k+1-i);
end
else
for i = 2:1:k
rema = rema + bino_fir(1,i)*X_esti(1,k+1-i);
end
end
X_pre = diff_X_esti - rema + q; %一步状态预测
%预测误差协方差矩阵:P_pred
%对误差矩阵进行cholsky分解
S_chol = chol(P_xesti(1,k-1))';
%计算余项
rema_P = 0;
if k>L+1
for i = 2:1:L+2
rema_P = rema_P + bino_fir(1,i)*P_xesti(1,k+1-i)*bino_fir(1,i)';
end
else
for i = 2:1:k
rema_P = rema_P + bino_fir(1,i)*P_xesti(1,k+1-i)*bino_fir(1,i)';
end
end
%临时变量 temp_fun : 函数差值,函数为单变量
temp_fun = f(X_esti(1,k-1)+h_con*S_chol) - f(X_esti(1,k-1)-h_con*S_chol);
temp = 1/(4*h_con^2) * temp_fun^2 + rema_P + ...
1/h_con*0.5*temp_fun*S_chol'*(-bino_fir(1,2))' + ...
1/h_con*(-bino_fir(1,2))*S_chol*0.5*temp_fun';
P_xpred = temp + Q;
%测量值估计 Z_esti ---- Z_k|k-1
Z_esti = h(X_pre) + r;
%测量预测误差协方差矩阵:P_zpred ---- P_z_k|k-1
P_zpred = P_xpred + R;
%计算卡尔曼增益:Kk(2*1)
Kk = P_xpred/P_zpred;
%状态更新
X_esti(1,k) = X_pre + Kk*( Z_meas(1,k) - Z_esti );
%估计方差矩阵更新
P_xesti(1,k) = P_zpred - Kk*P_zpred*Kk';
end
%输入与测量输出图
k = 1:1:N;
LineWidth = 1.5;
figure;
plot(k,X_real(1,:),'r',k,X_esti(1,:),'b--','linewidth',LineWidth);
set(gcf,'Position',[200 200 400 300]);
% axis([xmin xmax ymin ymax]) 设置坐标轴在指定的区间
axis normal
axis([0 N -6 6 ])
ylabel('x','FontSize',8)
xlabel('time(sec)','FontSize',8)
% 设置坐标轴刻度字体名称,大小
set(gca,'FontName','Helvetica','FontSize',8)
legend('real state','estimated state','Location','best');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 分数阶卡尔曼滤波器仿真复现
% 论文: fractional order EKF
% 目的:FCDKF与FEKF的性能比较
% 函数实验: D^{0.7} x_k = 3*sin(2*x_{k-1}) -x_{k-1} + w_k
% y_k = x_k + v_k
% 结果:
%
% 备注:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc
clear
%仿真步长
N = 50;
q = 1; %系统噪声均值
r = 1; %测量噪声均值
Q = 0.81; %系统噪声方差矩阵
R = 0.25; %测量噪声方差矩阵
%GL定义下短记忆原理的长度
L = N+1;
%计算alpha阶次对应的GL定义系数 binomial coefficient
bino_fir = zeros(1,N); %微分阶次为0.7时GL定义下的系数
alpha = 0.7;
bino_fir(1,1) = 1;
for i = 2:1:N
bino_fir(1,i) = (1-(alpha+1)/(i-1))*bino_fir(1,i-1);
end
%系统矩阵设置
I = eye(1,1); %生成单位阵
%状态测量初始化
X_real = zeros(1,N); %真实状态
Z_meas = zeros(1,N); %实际观测值
%噪声
W_noise = sqrt(Q)*randn(1,N) + q; %系统噪声
V_noise = sqrt(R)*randn(1,N) + r; %测量噪声
x_0 = 0; %初始状态
X_real(1,1) = x_0; %真实状态初始值
Z_meas(1,1) = V_noise(1,1); %测量数据初始值
% 系统函数与测量函数
f=@(x)3*sin(2*x)-x;
h=@(x)x;
for k=2:1:N
%计算实际状态
diff_X_real = f(X_real(1,k-1)) + W_noise(1,k-1);
rema = 0;
for i = 2:1:k
rema = rema + bino_fir(1,i)*X_real(1,k+1-i);
end
X_real(1,k) = diff_X_real - rema;
%实际观测值
Z_meas(1,k) = h(X_real(1,k)) + V_noise(1,k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------分数阶扩展卡尔曼滤波器性能测试---------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X_esti = zeros(1,N); %状态最优估计值
P_xesti = zeros(1,N); %估计误差方差阵
%初始值设置(初始矩阵不能为零)
P_pred_0 = 100; %初始预测方差阵
P_xesti(1,1) = P_pred_0; %初始估计方差阵
for k=2:1:N
%卡尔曼滤波
%状态预测:X_pre
diff_X_esti = f(X_esti(1,k-1));
%计算余项
rema = 0;
if k>L
for i = 2:1:L+1
rema = rema + bino_fir(1,i)*X_esti(1,k+1-i);
end
else
for i = 2:1:k
rema = rema + bino_fir(1,i)*X_esti(1,k+1-i);
end
end
X_pre = diff_X_esti - rema + q; %一步状态预测
%预测误差协方差矩阵:P_pred
%计算余项
rema_P = 0;
if k>L+1
for i = 3:1:L+2
rema_P = rema_P + bino_fir(1,i)*P_xesti(1,k+1-i)*bino_fir(1,i)';
end
else
for i = 3:1:k
rema_P = rema_P + bino_fir(1,i)*P_xesti(1,k+1-i)*bino_fir(1,i)';
end
end
F = 6*cos(2*X_esti(1,k-1)) - 1;
P_xpred = (F-bino_fir(1,2))*P_xesti(1,k-1)*(F-bino_fir(1,2))'+ Q + rema_P;
%测量值估计 Z_esti ---- Z_k|k-1
Z_esti = h(X_pre) + r;
%计算卡尔曼增益:Kk(2*1)
H = 1;
Kk = P_xpred*H'/(H*P_xpred*H' + R);
%状态更新
X_esti(1,k) = X_pre + Kk*( Z_meas(1,k) - Z_esti );
%估计方差矩阵更新
P_xesti(1,k) = (I-Kk*H)*P_xpred;
end
%输入与测量输出图
k = 1:1:N;
LineWidth = 1.5;
figure;
plot(k,X_real(1,:),'r',k,X_esti(1,:),'b--','linewidth',LineWidth);
set(gcf,'Position',[200 200 400 300]);
% axis([xmin xmax ymin ymax]) 设置坐标轴在指定的区间
axis normal
axis([0 N -6 6 ])
ylabel('x','FontSize',8)
xlabel('time(sec)','FontSize',8)
% 设置坐标轴刻度字体名称,大小
set(gca,'FontName','Helvetica','FontSize',8)
legend('real state','estimated state','Location','best');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 分数阶粒子滤波仿真复现
% 论文: fractional order PF
% 目的:分数阶粒子滤波算法测试
% 对系统噪声均值进行估计
% 函数实验: D^{0.7} x_k = 3*sin(2*x_{k-1}) -x_{k-1} + w_k
% y_k = x_k + v_k
% 结果:较好的对状态进行估计,常值系统噪声均值收敛
%
% 备注:分数阶粒子滤波的算法测试
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear all;
LineWidth = 1.5;
h_con = sqrt(3);
tf = 100; % 仿真时长
N = 100; % 粒子个数
%系统矩阵设置
% A = [0,1; -0.1,-0.2]; %系统矩阵
% B = [0; 1]; %
% C = [0.1,0.3]; %
I = eye(1,1); %生成单位阵
%I(3,3) = 0;
%噪声
q = 0; %系统噪声均值
r = 0; %测量噪声均值
Q = 0.81; %系统噪声方差矩阵
R = 0.25; %测量噪声方差矩阵
W_noise = sqrt(Q)*randn(1,N) + q; %系统噪声
V_noise = sqrt(R)*randn(1,N) + r; %测量噪声
x = zeros(1,tf); % 系统状态真实值 初始值0
y = zeros(1,tf); % 系统状态真实值 初始值0
y(1,1) = x(1,1) + sqrt(R) * randn;
P = zeros(1,tf); % 采样方差
P(1,1) = 2; % 初始采样分布的方差
xhatPart = zeros(1,tf); %状态估计值
xpart = zeros(N,tf);
for i = 1 : N
xpart(i,1) = x(1,1) + sqrt(P(1,1)) * randn; %初始状态服从x=0均值,方差为sqrt(P)的高斯分布
end
% xArr = [x];
% yArr = [];
% xhatArr = [x];
% PArr = [P];
%xhatPartArr = [xhatPart]; %
%计算alpha阶次对应的GL定义系数 binomial coefficient
bino_fir = zeros(1,N); %微分阶次为0.7时GL定义下的系数
alpha = 1;
bino_fir(1,1) = 0.7;
for i = 2:1:N
bino_fir(1,i) = (1-(alpha+1)/(i-1))*bino_fir(1,i-1);
end
%%
% diff_X_real 表示k时刻状态的微分
diff_X_real = 0;
%% 开始循环
for k = 2 : tf
diff_X_real = 3*sin(2*x(1,k-1)) -x(1,k-1) + W_noise(1,k-1);
rema = 0;
for i = 2:1:k
rema = rema + bino_fir(1,i)*x(1,k+1-i);
end
x(1,k) = diff_X_real - rema;
%k时刻真实值
y(1,k) = x(1,k) + V_noise(1,k); %k时刻观测值
%% 采样N个粒子
for i = 1 : N
%采样获得N个粒子
xpartminus(i) = 3*sin(2*xpart(i,k-1)) - xpart(i,k-1) + sqrt(Q) * randn;
temp = 0;
for j = 2 : 1 : k
temp = temp + bino_fir(1,j)*xpart(i,k+1-j);
end
xpartminus(i) = xpartminus(i) - temp;
ypart = xpartminus(i); %每个粒子对应观测值
vhat = y(1,k) - ypart; %与真实观测之间的似然
q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
%每个粒子的似然即相似度
end
%%
%权值归一化
qsum = sum(q);
for i = 1 : N
q(i) = q(i) / qsum; %归一化后的权值 q
end
%%
%根据权值重新采样
for i = 1 : N
u = rand;
qtempsum = 0;
for j = 1 : N
qtempsum = qtempsum + q(j);
if qtempsum >= u
xpart(i,k) = xpartminus(j);
break;
else
xpart(i,k) = xpart(i,k-1);
end
end
end
xhatPart(1,k) = mean(xpart(:,k));
%%
%最后的状态估计值即为N个粒子的平均值,这里经过重新采样后各个粒子的权值相同
% xArr = [xArr x];
% yArr = [yArr y];
% % xhatArr = [xhatArr xhat];
% PArr = [PArr P];
% xhatPartArr = [xhatPartArr xhatPart];
end
%%
t = 1 : tf;
figure;
plot(t, x, 'r', t, xhatPart, 'b--','linewidth',LineWidth);
Esitimated_state = legend('Real Value','Estimated Value','Location','best');
set(Esitimated_state,'Interpreter','latex')
set(gcf,'Position',[200 200 400 300]);
axis([0 50 -6 6]) %设置坐标轴在指定的区间
axis normal
set(gca,'FontSize',10);
xlabel('time step','FontSize',7);
ylabel('state','FontSize',7);
%设置坐标轴刻度字体名称,大小
set(gca,'FontName','Helvetica','FontSize',8)
%title('Fractional particle filter')
%xhatRMS = sqrt((norm(x - xhat))^2 / tf);
%xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
% figure;
% plot(t,abs(x-xhatPart),'b');
% title('The error of FPF')
%%
% t = 0 : tf;
% figure;
% plot(t, xArr, 'b-.', t, xhatPartArr, 'k-');
% legend('Real Value','Estimated Value');
% set(gca,'FontSize',10);
% xlabel('time step');
% ylabel('state');
% title('Particle filter')
% xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
% xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
% figure;
% plot(t,abs(xArr-xhatPartArr),'b');
% title('The error of PF')
%*************************************************************************%
% 分数阶粒子滤波仿真复现
% 论文: fractional order PF
% 目的:分数阶粒子滤波算法测试
% 对系统噪声均值进行估计
% 函数实验: D^{0.7} x_k = 3*sin(2*x_{k-1}) -x_{k-1} + w_k
% y_k = x_k + v_k
% 结果:较好的对状态进行估计
%
% 备注:分数阶粒子滤波的算法测试
% 随机重采样
%*************************************************************************%
clc;
clear all;
LineWidth = 1.5;
SimuTimes = 100; % 仿真时长
NumParticle = 100; % 粒子个数
%系统矩阵设置
I = eye(1,1); % 生成单位阵
%噪声
q = 0; % 系统噪声均值
r = 0; % 测量噪声均值
Q = 0.81; % 系统噪声方差矩阵
R = 0.25; % 测量噪声方差矩阵
W_noise = sqrt(Q)*randn(1,SimuTimes) + q; % 系统噪声
V_noise = sqrt(R)*randn(1,SimuTimes) + r; % 测量噪声
X_RealState = zeros(1,SimuTimes); % 系统状态真实值 初始值0
Y_RealMeas = zeros(1,SimuTimes); % 系统状态真实值 初始值0
Y_RealMeas(1,1) = X_RealState(1,1) + sqrt(R) * randn;
P_SampleCov = zeros(1,SimuTimes); % 采样方差
x_EstiState = zeros(1,SimuTimes); % 状态估计值
P_SampleCov(1,1) = 2; % 初始采样分布的方差
ParticleWeight = zeros(1,NumParticle); % 初始化权重
x_SamplePart_temp = zeros(1,NumParticle); % 中间变量
x_SampleParticle = zeros(NumParticle,SimuTimes);
% Intinialization particle, prior distirbution p(x_0)
for i = 1 : NumParticle
% 初始状态服从 x=0 均值,方差为 sqrt(P) 的高斯分布
x_SampleParticle(i,1) = x_EstiState(1,1) + q + sqrt(P_SampleCov(1,1)) * randn;
end
% xArr = [x];
% yArr = [];
% xhatArr = [x];
% PArr = [P];
% xhatPartArr = [xhatPart]; %
f = @(x)3*sin(2*x) - x;
h = @(x)x;
% 计算alpha阶次对应的GL定义系数 binomial coefficient
bino_fir = zeros(1,SimuTimes); % 微分阶次为0.7时GL定义下的系数
alpha = 0.7;
bino_fir(1,1) = 1;
for i = 2:1:NumParticle
bino_fir(1,i) = (1-(alpha+1)/(i-1))*bino_fir(1,i-1);
end
%%
% diff_X_real 表示k时刻状态的微分
diff_X_real = 0;
%% 计算实际状态 calculate real state and measurement
for k = 2 : SimuTimes
diff_X_real = f(X_RealState(1,k-1)) + W_noise(1,k-1);
rema = 0;
for i = 2:1:k
rema = rema + bino_fir(1,i) * X_RealState(1,k+1-i);
end
X_RealState(1,k) = diff_X_real - rema;
% k 时刻真实值
Y_RealMeas(1,k) = h(X_RealState(1,k)) + V_noise(1,k); % k 时刻观测值
%% 采样N个粒子
for i = 1 : NumParticle
% Draw particle: x^i_k ~ p(x_k | x^i_k-1) state transform function
% 采样获得 Num_particle 个粒子
x_SamplePart_temp(1,i) = f(x_SampleParticle(i,k-1)) + q + sqrt(Q) * randn;
temp = 0;
for j = 2 : 1 : k
temp = temp + bino_fir(1,j)*x_SampleParticle(i,k+1-j);
end
x_SamplePart_temp(1,i) = x_SamplePart_temp(1,i) - temp;
y_ParticleMeas = h(x_SamplePart_temp(1,i)) + r; % 每个粒子对应的观测值
ErrorMeas = Y_RealMeas(1,k) - y_ParticleMeas; % 与真实观测之间的似然
% Draw weight: w^i_k ~ p(z_k | x^i_k) measurement transform function
% 粒子权值,与测量方程有关
%ParticleWeight(1,i) = h(x_SamplePart_temp(1,i)) + r + sqrt(R) * randn;
ParticleWeight(1,i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-ErrorMeas^2 / 2 / R);
% 每个粒子的似然即相似度
end
%%
% 权值归一化
weight_sum = sum(ParticleWeight);
for i = 1 : NumParticle
ParticleWeight(1,i) = ParticleWeight(1,i) / weight_sum; % 归一化后的权值 q
end
% 根据权值重新采样 随机重采样算法
qtempsum = zeros(1,NumParticle);
qtempsum(1,1) = ParticleWeight(1,1);
for i = 2 : 1 : NumParticle
qtempsum(1,i) = qtempsum(1,i-1) + ParticleWeight(1,i);
end
for i = 1 : NumParticle
UniRandom = rand; % 产生均匀分布随机数
for j = 1 : NumParticle
% 累计权值
%qtempsum = qtempsum + ParticleWeight(1,j);
if qtempsum(1,j) >= UniRandom
x_SampleParticle(i,k) = x_SamplePart_temp(1,j);
break;
%else
% x_SampleParticle(i,k) = x_SampleParticle(i,k-1);
end
end
end
%%
% 根据权值重新采样
% c_Weight = zeros(1,NumParticle);