扩展卡尔曼滤波，非线性EKF的仿真与用法

% EKF clear; x = 0.1; % 初始状态 x_estimate = 1;%状态的估计 e_x_estimate = x_estimate; %EKF的初始估计 Q = 10;%input('请输入过程噪声方差Q的值: '); % 过程状态协方差 R = 1;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差 P =5;%初始估计方差 tf = 50; N = 500; e_P = P; %EKF方差 x_array = [x];%真实值数组 e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组 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; %观测值 %扩展卡尔曼滤波器 %进行估计 第一阶段的估计 e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate^2) + 8 * cos(1.2*(k-1)); %Xk,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; %Pk,k-1 %扩展卡尔曼增益 e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R); %K %进行估计值的更新 第二阶段 e_x_estimate_2 = e_x_estimate_1 + e_K * (y - e_y_estimate); %Xk %更新后的估计值的误差 e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate; %Pk %进入下一次迭代的参数变化 e_P = e_p_estimate_update; % e_P 初值 e_x_estimate = e_x_estimate_2; % e_x_estimate 初值 %进行画图程序 x_array = [x_array,x]; e_x_estimate_array = [e_x_estimate_array,e_x_estimate]; e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k)); end t = 0 : tf; figure; plot(t,x_array,'k.',t,e_x_estimate_array,'r-'); set(gca,'FontSize',10); set(gcf,'color','White'); xlabel('时间步长');% lable --->label 我的神 ylabel('状态'); legend('真实值','EKF估计值'); t = 1 : tf; figure; plot(t,e_error,'r-'); set(gca,'FontSize',10); set(gcf,'color','White'); xlabel('时间步长');% lable --->label 我的神 ylabel('状态'); legend('EKF估计值误差'); % toc;