MATLAB实现扩展卡尔曼滤波和无味卡尔曼滤波

以下是扩展卡尔曼滤波和无味卡尔曼滤波对直线x=2*k+2进行滤波，怎么改状态方程和传递矩阵，跪求大神指导，谢谢了。
% EKF UKF  算法
clear;
x = 0.1; % 初始状态
x_estimate = 1;%状态的估计
e_x_estimate = x_estimate;  %EKF的初始估计
u_x_estimate = x_estimate;  %UKF的初始估计
Q = 10;% % 过程状态协方差
R = 1;% % 测量噪声协方差
P =5;%初始估计方差
e_P = P; %EKF方差
u_P = P;%UKF方差
tf = 50; % 模拟长度
x_array = [x];%真实值数组
e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组
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 =  (2*(k-1)+2) + sqrt(Q) * randn; %状态值
    y = (x^2 / 20) + sqrt(R) * randn; %观测值
    %扩展卡尔曼滤波器
    %进行估计  第一阶段的估计
    e_x_estimate_1 =2*(k-1)+2;%1 状态估计
    e_y_estimate =  (e_x_estimate_1)^2/20;%观测估计 %这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的  第24行的y也是观测值  不过是由加了噪声的真实值得到的
    %相关矩阵
    e_A =1;%传递矩阵
    e_H = e_x_estimate_1; %观测矩阵
    %估计的误差
    e_p_estimate =  e_A * e_P * e_A' + Q;%2 协方差估计
    %扩展卡尔曼增益
    e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R);%3
    %进行估计值的更新  第二阶段
    e_x_estimate_2 = e_x_estimate_1 + e_K * (y - e_y_estimate);%4
    %更新后的估计值的误差
    e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate;%5
    %进入下一次迭代的参数变化
    e_P = e_p_estimate_update;
    e_x_estimate = e_x_estimate_2;
%    无味卡尔曼滤波器
   %采样点的选取 存在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 = u_k/(2 * (u_symmetry_number+u_k));
    %把这些粒子通过传递方程 得到下一个状态
    for i = 1: u_total_number
        u_x_par(i) =2*(k-1)+2;
    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_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;
    %进行画图程序
    x_array = [x_array,x];
    e_x_estimate_array = [e_x_estimate_array,e_x_estimate];
    u_x_estimate_array = [u_x_estimate_array,u_x_estimate];
    e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k));
    u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k));
end
    t = 0 : tf;
    figure(1);
    plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,u_x_estimate_array,'b:');
    set(gca,'FontSize',10);
    set(gcf,'color','White');
    xlabel('时间步长');% lable --->label 我的神
    ylabel('状态');
    legend('真实值','EKF估计值','UKF估计值');
    e_xrms = sqrt((norm(x_array-e_x_estimate_array)^2)/tf);
    disp(['EKF估计误差均方值=',num2str(e_xrms)]);
    u_xrms = sqrt((norm(x_array-u_x_estimate_array)^2)/tf);
    disp(['UKF估计误差均方值=',num2str(u_xrms)]);
    t = 1 : tf;
    figure(2);
    plot(t,e_error,'r-',t,u_error,'b:');
    set(gca,'FontSize',10);
    set(gcf,'color','White');
    xlabel('时间步长');% lable --->label 我的神
    ylabel('状态');
    legend('EKF估计值误差','UKF估计值误差');