【状态估计】扩展卡尔曼滤波器EKF和非线性优化两种方法在状态估计中的表现附matlab代码

✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。 往期回顾关注个人主页完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍这段代码实现了一个状态估计的模拟对比了扩展卡尔曼滤波器EKF和非线性优化通过lsqnonlin函数实现两种方法在状态估计中的表现。以下是对代码原理和背景的详细介绍1. 背景在许多工程和科学应用中需要根据一系列带有噪声的观测数据来估计系统的状态。例如在机器人定位、传感器融合等场景下由于测量设备的误差以及环境干扰直接获取准确的系统状态是困难的因此需要借助状态估计方法来尽可能准确地推断系统的真实状态。2. 代码结构与原理初始化部分rng(1);state_gt [0;0]; state_ekf state_gt[1;1]; state_cov_ekf eye(2); state_nonlin state_gt[1;1]; state_cov_nonlin eye(2); observations zeros(2,0);rng(1)设置随机数生成器的种子为 1以确保每次运行代码时生成的随机数序列相同便于结果的可重复性。state_gt定义系统的真实状态这里初始化为[0; 0]表示二维空间中的原点。state_ekf和state_cov_ekf分别初始化扩展卡尔曼滤波器EKF的估计状态和协方差矩阵。估计状态初始值在真实状态的基础上加上[1; 1]协方差矩阵初始化为 2x2 的单位矩阵eye(2)。state_nonlin和state_cov_nonlin类似地初始化非线性优化方法的估计状态和协方差矩阵初始值与 EKF 的设置相同。observations初始化观测数据矩阵初始时为空。循环部分for k1:10% Plot figure(1); clf; subplot(1,2,1); plot_state(state_gt, state_ekf, state_cov_ekf, observations); title(EKF); subplot(1,2,2); plot_state(state_gt, state_nonlin, state_cov_ekf, observations); title(Non - linear optimization); pause(0.5) % State prediction state_cov_ekf state_cov_ekfeye(2)*0.1; state_cov_nonlin state_cov_nonlineye(2)*0.1; % Generate observations Y_COUNT 5; observationszeros(2,Y_COUNT); observations_cov cell(Y_COUNT,1); for i1:Y_COUNT [z,z_cov] get_random_observation(state_gt); observations(:,i)z; observations_cov{i} z_cov; end % Update [state_ekf,state_cov_ekf] apply_ekf(state_ekf, state_cov_ekf, observations, observations_cov); [state_nonlin,state_cov_nonlin] apply_lsqnonlin(state_nonlin, state_cov_nonlin, observations, observations_cov); end绘图部分figure(1)创建或激活编号为 1 的图形窗口。clf清除当前图形窗口的内容。subplot(1,2,1)和subplot(1,2,2)将图形窗口划分为 1 行 2 列的子图并分别在第一个和第二个子图中进行绘制。plot_state函数用于绘制系统的真实状态、估计状态以及协方差矩阵对应的置信椭圆并展示观测数据。这里分别绘制 EKF 和非线性优化方法的状态估计结果。title为每个子图添加标题分别为 “EKF” 和 “Non - linear optimization”。pause(0.5)暂停程序执行 0.5 秒以便观察每次迭代后的绘图结果。状态预测部分state_cov_ekf state_cov_ekfeye(2)*0.1和state_cov_nonlin state_cov_nonlineye(2)*0.1在每次迭代中分别更新 EKF 和非线性优化方法的状态协方差矩阵。这里简单地将协方差矩阵增加一个小的噪声矩阵eye(2)*0.1模拟系统状态的不确定性随时间增加。生成观测部分Y_COUNT 5定义每次迭代生成的观测数据数量为 5。observationszeros(2,Y_COUNT)初始化观测数据矩阵大小为 2x5每列代表一个二维观测数据。observations_cov cell(Y_COUNT,1)初始化一个包含 5 个元素的单元格数组用于存储每个观测数据的协方差矩阵。在内部循环中通过get_random_observation函数生成基于真实状态state_gt的随机观测数据z及其协方差矩阵z_cov并将其存储到observations和observations_cov中。更新部分[state_ekf,state_cov_ekf] apply_ekf(state_ekf, state_cov_ekf, observations, observations_cov)调用apply_ekf函数使用 EKF 方法根据当前估计状态、协方差矩阵以及新生成的观测数据和其协方差矩阵更新 EKF 的估计状态和协方差矩阵。[state_nonlin,state_cov_nonlin] apply_lsqnonlin(state_nonlin, state_cov_nonlin, observations, observations_cov)调用apply_lsqnonlin函数使用非线性优化方法可能基于最小二乘原理通过lsqnonlin函数实现根据当前估计状态、协方差矩阵以及新生成的观测数据和其协方差矩阵更新非线性优化的估计状态和协方差矩阵。⛳️ 运行结果更多免费数学建模和仿真教程关注领取