【状态估计】【卡尔曼滤波kalman】UKF、EKF、粒子滤波PF学习与研究（Matlab代码实现）

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。当哲学课上老师问你什么是科学，什么是电的时候，不要觉得这些问题搞笑。哲学是科学之母，哲学就是追究终极问题，寻找那些不言自明只有小孩子会问的但是你却回答不出来的问题。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能让人胸中升起一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它居然给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

    或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

卡尔曼滤波（Kalman Filter）是一种用于估计系统状态的强大工具，适用于线性动态系统和观测的高斯噪声的情况。但是，对于非线性系统和非高斯噪声的情况，卡尔曼滤波可能表现不佳。为了处理这些情况，人们开发了一些扩展的滤波方法，其中最常见的包括扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）。

1. **扩展卡尔曼滤波（EKF）**：EKF 是对卡尔曼滤波的一种扩展，用于处理非线性系统。它通过在线性化状态转移和观测方程来逼近非线性系统的动态。EKF通过在每个时间步骤中使用雅可比矩阵来线性化系统模型和观测模型，然后应用卡尔曼滤波的递归步骤来进行状态估计。尽管EKF在一定程度上解决了非线性系统的问题，但由于线性化可能不准确，因此在高度非线性的系统中可能表现不佳。

2. **无迹卡尔曼滤波（UKF）**：UKF是另一种用于处理非线性系统的卡尔曼滤波扩展。UKF通过选择一组称为Sigma点的状态样本来近似状态的概率分布。这些Sigma点经过非线性变换，然后通过卡尔曼滤波的步骤进行估计。与EKF相比，UKF在处理非线性系统时更加准确，因为它避免了对系统进行线性化。

3. **粒子滤波（PF）**：粒子滤波是一种基于蒙特卡洛方法的滤波技术，适用于非线性系统和非高斯噪声的情况。在粒子滤波中，状态的后验概率分布通过一组随机样本（粒子）来近似表示。这些粒子根据系统的动态模型和观测模型进行预测和更新。由于粒子滤波直接对后验概率进行采样，因此它能够处理非线性系统和非高斯噪声的情况，并且不需要对模型进行线性化。

每种滤波方法都有其优缺点，并且适用于不同类型的系统和应用。选择合适的滤波方法通常取决于系统的非线性程度、观测的噪声特性以及计算资源的可用性。

以下是对卡尔曼滤波（Kalman Filter）及其扩展方法，包括扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）的学习与研究文档：

一、卡尔曼滤波（Kalman Filter）

1. 概述

• 卡尔曼滤波是一种用于估计系统状态的数学方法，它结合了传感器测量和系统模型的信息，以获得对系统状态的最佳估计。
• 卡尔曼滤波器的设计理念基于线性代数和概率论，它采用了一种递归的估计方法，通过不断融合新的观测数据和先前的估计值，来优化对系统状态的预测。

2. 基本原理

• 卡尔曼滤波的核心在于其五个关键公式，这些公式共同构成了滤波器的理论基础和计算框架。
• 预测步骤：根据系统的动态模型，使用先前的状态估计来预测下一个时刻的状态，并估计状态的不确定性。
• 更新步骤：将新的测量值与预测值进行比较，根据它们的不确定性，更新状态的估计值和不确定性。这一步使用贝叶斯推断来融合测量和预测信息。

3. 应用

• 卡尔曼滤波在航空航天、自动控制、信号处理、金融预测等众多领域得到了广泛应用。
• 在这些领域中，系统状态的精确估计是实现有效控制的关键，而卡尔曼滤波正是一种优秀的状态估计算法。

二、扩展卡尔曼滤波（EKF）

1. 概述

• EKF是对卡尔曼滤波的一种扩展，用于处理非线性系统。
• 它通过在线性化状态转移和观测方程来逼近非线性系统的动态。

2. 基本原理

• EKF在每个时间步骤中使用雅可比矩阵来线性化系统模型和观测模型，然后应用卡尔曼滤波的递归步骤来进行状态估计。

3. 局限性

• 尽管EKF在一定程度上解决了非线性系统的问题，但由于线性化可能不准确，因此在高度非线性的系统中可能表现不佳。

三、无迹卡尔曼滤波（UKF）

1. 概述

• UKF是另一种用于处理非线性系统的卡尔曼滤波扩展。
• 它通过选择一组称为Sigma点的状态样本来近似状态的概率分布。

2. 基本原理

• 这些Sigma点经过非线性变换，然后通过卡尔曼滤波的步骤进行估计。
• 与EKF相比，UKF在处理非线性系统时更加准确，因为它避免了对系统进行线性化。

四、粒子滤波（PF）

1. 概述

• 粒子滤波是一种基于蒙特卡洛方法的滤波技术，适用于非线性系统和非高斯噪声的情况。

2. 基本原理

• 在粒子滤波中，状态的后验概率分布通过一组随机样本（粒子）来近似表示。
• 这些粒子根据系统的动态模型和观测模型进行预测和更新。
• 由于粒子滤波直接对后验概率进行采样，因此它能够处理非线性系统和非高斯噪声的情况，并且不需要对模型进行线性化。

3. 优点

• 粒子滤波具有强大的非线性处理能力，适用于复杂系统的状态估计。

五、比较与总结

1. 比较

• EKF适用于弱非线性系统，但在高度非线性系统中可能表现不佳。
• UKF通过非线性变换和Sigma点近似，提高了对非线性系统的处理能力。
• PF基于蒙特卡洛方法，具有强大的非线性处理能力，但计算复杂度较高。

2. 总结

• 卡尔曼滤波及其扩展方法（EKF、UKF、PF）在状态估计领域发挥着重要作用。
• 选择合适的滤波方法通常取决于系统的非线性程度、观测的噪声特性以及计算资源的可用性。

综上所述，卡尔曼滤波及其扩展方法（EKF、UKF、PF）在状态估计领域具有广泛的应用前景和重要的研究价值。在实际应用中，应根据系统的特性和需求选择合适的滤波方法。

📚2 运行结果

2.1 UKF在目标跟踪中的应用

编辑

编辑

2.2 EKF

编辑

编辑

2.3 EKF standard

编辑

编辑

2.4 UKF

编辑

编辑

2.5 粒子滤波PF

编辑

编辑

2.6 EKF、UKF、PF三个算法比较

编辑

编辑

编辑

编辑

部分代码：

%粒子滤波pf

tic;

Xpf=PF(Xpf,Z,Q,R,M,N);  

toc;

tic;

Xukf=UKF(Xukf,Z,N+1,Q,R);

toc;

for i=1:N+1

   Err_Obs(i)=RMS(X(:,i),Z(:,i));%滤波前的误差

   Err_UKF(i)=RMS(X(:,i),Xukf(:,i));%滤波后的误差

   Err_PF(i)=RMS(X(:,i),Xpf(:,i));%滤波后的误差

end

mean_Obs=mean(Err_Obs);

mean_UKF=mean(Err_UKF);

mean_PF=mean(Err_PF);

%%%%%%%%%%%

%画图

figure

hold on ;box on

t=(0:1:N);

plot(t,X(1,:),'-r','LineWidth',1);%实际轨迹

plot(t,Xukf(1,:),'-.k','LineWidth',1);%卡尔曼滤波轨迹

plot(t,Xpf(1,:),'-.ob','LineWidth',1);%观测轨迹

% plot(X(1,:),X(3,:),'-k.');

legend('真实轨迹','UKF轨迹','PF轨迹');

xlabel('横坐标 T/s');

ylabel('纵坐标 X/m');

figure

hold on;box on;

plot(t,Err_Obs,'-');

plot(t,Err_UKF,'--');

plot(t,Err_PF,'-.');

% legend('滤波前误差',num2str(mean_Observation),'基本滤波后误差','固定增益滤波后误差');

legend(sprintf('滤波前误差%.03f',mean_Obs),sprintf('UKF滤波后误差%.03f',mean_UKF),sprintf('PF滤波后误差%.03f',mean_PF));

xlabel('观测时间/s');

ylabel('误差值');

%%%%%%%%%%%%%

%子函数

% 计算欧氏距离子函数

function dist=RMS(X1,X2)

if length(X2)<=2

   dist=sqrt((X1(1)-X2(1))^2);

else

   dist=sqrt((X1(1)-X2(1))^2);

end

end

function res=gfun(Xekf,t)

res= 0.5*Xekf + 25*Xekf/(1 + Xekf^2) + 8*cos(0.4*(t));

end

function res=hfun(X,k)

res=X^2/20;

end

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]贾飞飞.非线性滤波算法及其应用研究[D].电子科技大学,2016.

[2]李彩菊,李亚安.扩展卡尔曼滤波和粒子滤波算法性能比较研究[C]//2009年中国西部地区声学学术交流会.2009.

[3]钱翔.基于改进粒子滤波器目标跟踪算法研究[D].安徽大学[2024-04-24].DOI:10.7666/d.d158850.

[4]朱瑞奇,张胜修,孙巧,等.基于Kalman预测重要性建议分布的粒子滤波视觉跟踪算法[J].兵器装备工程学报, 2013, 34(010):109-113.DOI:10.11809/scbgxb2013.10.032.

🌈4 Matlab代码实现

资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取【请看主页然后私信】