粒子滤波程序.docxVIP

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 + xA2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态值 y = (xA2 / 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 + xA2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %犬态值 y = (xA2 / 20) + sqrt(R) * randn; %观测值 %扩展卡尔曼滤波器 %进行估计第一阶段的估计 e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimateA2) + 8 * cos(1.2*(k-1)); e_y_estimate = (e_x_estimate_1)A2/20; %这是根据k=1时估计值为1得到的观测 值；只是这个由我估计得到的 第24行的y也是观测值 不过是由加了噪声的真实值得到 的 %相关矩阵 e_A = linear + 25 * (1-e_x_estimateA2)/((1+e_x_estimateA2)A2);%^^ 递矩