UKF.rar

UKF.rar

% UKF算法
clear;
% tic;
x = 0; % 初始状态 
x_estimate = 1;%状态的估计
u_x_estimate = x_estimate;  %UKF的初始估计
Q = 10; % 过程状态协方差 
R =1; % 测量噪声协方差 
P =5;%初始估计方差
u_P = P;%UKF方差
tf = 50; % 模拟长度 
x_array = [x];%真实值数组
u_x_estimate_array = [u_x_estimate];%UKF最优估计值数组
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）） + sqrt（Q） * randn; %状态值 
    y = （x^2 / 20） + sqrt（R） * randn; %观测值
    
    
    %不敏卡尔曼滤波器
    
   %采样点的选取 存在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.5 * u_x_par（i） + 25 * u_x_par（i）/（1+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
    
%    %对传递后的均值和方差进行采样  产生粒子 存在y（i）

%另外存在y_2obser（i） 中；
    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/20;
    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;
  

    %进行画图程序 
    x_array = [x_arrayx];
    u_x_estimate_array = [u_x_estimate_arrayu_x_estimate];
    
    u_error（k:） = abs（x_array（k）-u_x_estimate_array（k））;
 
end
    t = 0 : tf;
    figure;
    plot（tx_array‘k.‘tu_x_estimate_array‘b:‘）;
    set（gca‘FontSize‘10）;
    set（gcf‘color‘‘Yellow‘）;
    xlabel（‘time step‘

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3845  2012-03-15 21:25  UKF.m

----------- ---------  ---------- -----  ----

                 3845                    1