• 大小: 8KB
    文件类型: .m
    金币: 1
    下载: 0 次
    发布日期: 2021-05-09
  • 语言: Matlab
  • 标签: EKFUKF  

资源简介

EKF,UKF程序,程序是关于EKF,UKF对一组数据处理结果的比较

资源截图

代码片段和文件信息

% EKF UKF PF 的三个算法
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; %EKF方差
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;
    %进入下一次迭代的参数变化 
    e_P = e_p_estimate_update;
    e_x_estimate = e_x_estimate_2;        
   %%%%%%%%%%%%%%%%%%%%%%%%%%%粒子滤波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
          for i = 1 : N 
        p_xpartminus(i) = 0.5 * p_xpart(i) + 25 * p_xpart(i) / (1 + p_xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %这个式子比下面一行的效果好
%        xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1));
        p_ypart = p_xpartminus(i)^2 / 20; %预测值 
        p_vhat = y - p_ypart;% 观测和预测的差 
        p_q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-p_vhat^2 / 2 / R); %各个粒子的权值
    end 
    % 平均每一个估计的可能性 
    p_qsum = sum(p_q); 
    for i = 1 : N 
        p_q(i) = p_q(i) / p_qsum;%各个粒子进行权值归一化 
    end 
   % 重采样 权重大的粒子多采点,权重小的粒子少采点 相当于每一次都进行重采样;
    for i = 1 : N 
        p_u = rand; 
        p_qtempsum = 0; 
        for j = 1 : N 
            p_qtempsum = p_qtempsum + p_q(j); 
            if p_qtempsum >= p_u 
          p_xpart(i) = p_xpartminus(j); %在这里 xpart(i) 实现循环赋值;终于找到了这里!!!
                break; 
            end 
        end 
    end     
    p_x_estimate = mean(p_xpart); 
%     p_x_estimate = 0;
%     for i = 1 : N
%         p_x_estimate =p_x_estimate + p_q(i)*p_xpart(i);
%     end
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%不敏卡尔曼滤波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

评论

共有 条评论