【状态估计】贝叶斯和卡尔曼滤波研究（Python&Matlab代码实现）

本文链接：https://blog.csdn.net/weixin_46039719/article/details/130374182

👨‍🎓个人主页：研学社的博客

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

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

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

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

2.1 KF

2.2 PF

2.3 Python实现

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

本文基本思路是：首先生成一个含有噪声的信号，然后对其进行KF滤波，使得噪声得到一定程度的削减，从而还原出原信号。具体实现中，使用了状态方程和观测方程，根据状态方程和观测方程来设定F、H、Q、R这四个矩阵，并通过计算KF滤波的五大公式来实现滤波。程序的最后一段是用不同颜色的线条展示出原信号、观测信号和滤波之后的信号，方便直观的比较三者的差异。

📚2 运行结果

2.1 KF

蓝色：滤波后

黄色：滤波后

2.2 PF

2.3 Python实现

部分代码：

%%%%%PF核心代码%%%%%
for i = 2:L
%预测步改变例子位置
for j = 1:L
xold(j) = sin(xold(j))+5*xold(j)/(xold(j)^2+1)+normrnd(0,0.1);
end
%更新步改变例子权重
for j = 1:L
%w1(i) = fR(y1-h(x1(i)))w0(i)
%此处fR假设为正态分布 R~(0,0.1)
%fR = (2*pi*R)^(-0.5)*exp(-((y(i)-xold(j)^2)^2/(2*R)))
w(j) = exp(-((y(i)-xold(j)^2)^2/(2*0.1)));
end
%归一化此处我使用两种方法进行w的归一化，方法二得到的w最终整体的滤波效果更好，why?
%%方法一
% for j = 1:L
% w(j) = w(j)/sum(w);
% end
%%方法二
w = w/sum(w);
% plot(t,w,'y',LineWidth=2)
%重采样
c = zeros(1,L);%每个粒子权重区间
c(1) = w(1);
for j = 2:L
c(j) = c(j-1)+w(j);
end
for j=1:L
a = unifrnd(0,1);%随机生成