粒子滤波程序

clear;

% tic;

x = 0.1; % 初始状态

x_estimate = 1;%状态的估计

e_x_estimate = x_estimate; %EKF的初始估计

u_x_estimate = x_estimate; %UKF的初始估计

p_x_estimate = x_estimate; %PF的初始估计

Q = 10;%input('请输入过程噪声方差Q的值: '); % 过程状态协方差

R = 1;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差

P =5;%初始估计方差

e_P = P; %UKF方差

u_P = P;%UKF方差

pf_P = P;%PF方差

tf = 50; % 模拟长度

x_array = [x];%真实值数组

e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组

u_x_estimate_array = [u_x_estimate];%UKF最优估计值数组

p_x_estimate_array = [p_x_estimate];%PF最优估计值数组

u_k = 1; %微调参数

u_symmetry_number = 4; % 对称的点的个数

u_total_number = 2 * u_symmetry_number + 1; %总的采样点的个数

linear = 0.5;

N = 500; %粒子滤波的粒子数

close all;

%粒子滤波初始N 个粒子

for i = 1 : N

p_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;

end

for k = 1 : tf

% 模拟系统

x = linear * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态值y = (x^2 / 20) + sqrt(R) * randn; %观测值

clear;

% tic;

x = 0.1; % 初始状态

x_estimate = 1;%状态的估计

e_x_estimate = x_estimate; %EKF的初始估计

u_x_estimate = x_estimate; %UKF的初始估计

p_x_estimate = x_estimate; %PF的初始估计

Q = 10;%input('请输入过程噪声方差Q的值: '); % 过程状态协方差

R = 1;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差

P =5;%初始估计方差

e_P = P; %UKF方差

u_P = P;%UKF方差

pf_P = P;%PF方差

tf = 50; % 模拟长度

x_array = [x];%真实值数组

e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组

u_x_estimate_array = [u_x_estimate];%UKF最优估计值数组

p_x_estimate_array = [p_x_estimate];%PF最优估计值数组

u_k = 1; %微调参数

u_symmetry_number = 4; % 对称的点的个数

u_total_number = 2 * u_symmetry_number + 1; %总的采样点的个数

linear = 0.5;

N = 500; %粒子滤波的粒子数

close all;

%粒子滤波初始N 个粒子

for i = 1 : N

p_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;

end

for k = 1 : tf

% 模拟系统

x = linear * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态值

y = (x^2 / 20) + sqrt(R) * randn; %观测值

%扩展卡尔曼滤波器

%进行估计第一阶段的估计

e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate^2) + 8 * cos(1.2*(k-1));

e_y_estimate = (e_x_estimate_1)^2/20; %这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的第24行的y也是观测值不过是由加了噪声的真实值得到的

%相关矩阵

e_A = linear + 25 * (1-e_x_estimate^2)/((1+e_x_estimate^2)^2);%传递矩阵

e_H = e_x_estimate_1/10; %观测矩阵

%估计的误差

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;