✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍一、引言在许多工程和科学领域如机器人导航、目标跟踪、信号处理等准确估计系统的状态至关重要。扩展卡尔曼滤波器EKF、无迹卡尔曼滤波器UKF和粒子滤波器PF是常用的状态估计算法。它们各自基于不同的原理在处理非线性系统时具有不同的特性。本文旨在深入比较这三种算法的性能帮助使用者根据具体应用场景选择最合适的算法。二、算法原理概述一扩展卡尔曼滤波器EKF线性化基础EKF 基于卡尔曼滤波器通过对非线性系统模型进行一阶泰勒展开线性化处理。对于非线性状态转移函数 f(xt,ut) 和观测函数 h(xt)在当前估计值附近进行线性化将非线性问题近似为线性问题从而可以应用卡尔曼滤波器的框架进行状态估计。核心步骤预测阶段利用线性化后的状态转移方程预测下一时刻的状态和协方差更新阶段通过线性化的观测方程结合实际观测值对预测状态进行修正得到更准确的估计值。二无迹卡尔曼滤波器UKFSigma 点采样UKF 不依赖于线性化而是通过采样一组 Sigma 点来近似系统的概率分布。这些 Sigma 点能够更准确地捕捉非线性函数的统计特性。通过将这些 Sigma 点通过非线性函数进行传递然后根据加权统计计算预测状态和协方差。优势原理相比于 EKFUKF 避免了复杂的雅可比矩阵计算线性化过程中的关键步骤并且在处理非线性问题时通常能提供更准确的估计因为它对概率分布的近似更精确。三粒子滤波器PF蒙特卡罗模拟PF 基于蒙特卡罗方法通过大量的随机样本粒子来表示系统的状态分布。每个粒子都有一个权重代表该粒子在当前状态估计中的可能性。随着新观测数据的到来粒子的权重根据观测模型进行更新同时粒子的位置也通过状态转移模型进行传播。灵活性优势PF 能够处理高度非线性和非高斯的系统因为它不依赖于对系统进行特定的数学近似而是通过大量粒子的分布来逼近真实状态分布具有很强的灵活性。三、性能比较维度一估计精度非线性程度影响在弱非线性系统中EKF 通常能提供较为准确的估计因为线性化近似带来的误差较小。然而随着系统非线性程度的增加EKF 的线性化误差逐渐增大估计精度显著下降。UKF 由于其 Sigma 点采样策略能更好地捕捉非线性系统的特性在中等非线性系统中表现优于 EKF。PF 在高度非线性系统中具有明显优势通过大量粒子对状态空间的广泛覆盖能更准确地估计状态。噪声特性影响对于高斯噪声环境三种滤波器理论上都能有效工作。但在非高斯噪声情况下PF 的表现更为出色因为它不依赖于噪声的特定分布假设而 EKF 和 UKF 通常基于高斯噪声假设进行设计在非高斯噪声下估计精度会受到较大影响。二计算复杂度EKF 计算量EKF 需要计算非线性函数的雅可比矩阵这在复杂系统中计算量较大。此外每次迭代都涉及矩阵运算包括矩阵乘法和求逆随着系统维度的增加计算复杂度呈指数增长。UKF 计算量UKF 虽然避免了雅可比矩阵的计算但需要采样和传播 Sigma 点每个 Sigma 点都要经过非线性函数运算计算量也较大。不过在一些情况下UKF 的计算效率可能优于 EKF特别是当非线性函数的雅可比矩阵计算非常复杂时。PF 计算量PF 的计算复杂度主要取决于粒子数量。为了获得准确的估计通常需要大量的粒子这导致计算量巨大尤其是在高维状态空间中。随着粒子数量的增加计算时间和内存需求都会显著上升。三收敛速度初始状态依赖EKF 和 UKF 的收敛速度在一定程度上依赖于初始状态估计的准确性。如果初始估计与真实状态相差较大可能需要较长时间才能收敛到准确估计。PF 相对来说对初始状态的依赖性较小因为大量粒子可以在状态空间中广泛搜索更快地接近真实状态分布。系统动态影响在系统状态变化较快的情况下EKF 和 UKF 可能由于线性化或 Sigma 点采样的局限性难以快速跟踪状态变化导致收敛速度变慢。PF 能够通过不断更新粒子权重和位置更灵活地适应系统动态变化在快速变化的系统中可能具有更快的收敛速度。⛳️ 运行结果 部分代码%本例子从百度文库中得到 稍加注释clearN200;%取200个数​%% 生成噪声数据 计算噪声方差wrandn(1,N); %产生一个1×N的行向量第一个数为0w为过程噪声其和后边的v在卡尔曼理论里均为高斯白噪声w(1)0;Qvar(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维方差与协方差相同)​vrandn(1,N);%测量噪声Rvar(v);​%% 计算真实状态x_true(1)0;%状态x_true初始值A1;%a为状态转移阵此程序简单起见取1for k2:Nx_true(k)A*x_true(k-1)w(k-1); %系统状态方程k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声此处忽略了系统的控制量end​​%% 由真实状态得到测量数据 测量数据才是能被用来计算的数据 其他都是不可见的H0.2;zH*x_truev;%量测方差c为量测矩阵同a简化取为一个数​​%% 开始 预测-更新过程​% x_predict: 预测过程得到的x% x_update:更新过程得到的x% P_predict:预测过程得到的P% P_update:更新过程得到的P​%初始化误差 和 初始位置x_update(1)x_true(1);%s(1)表示为初始最优化估计P_update(1)0;%初始最优化估计协方差​%% plot%作图红色为卡尔曼滤波绿色为量测蓝色为状态%kalman滤波的作用就是 由绿色的波形得到红色的波形 使之尽量接近蓝色的真实状态。t1:N;plot(t,x_update,r,t,z,g,t,x_true,b);legend(kalman,测量值,真值);xlabel(Iteration);​ 参考文献[1]于洪波,王国宏,孙芸,等.一种融合UKF和EKF的粒子滤波状态估计算法[J].系统工程与电子技术, 2013(07):29-33.DOI:10.3969/j.issn.1001-506X.2013.07.04.更多免费数学建模和仿真教程关注领取