贝叶斯滤波、卡尔曼滤波、粒子滤波学习手稿+MATLAB 实现
目录
1、贝叶斯滤波
1.1 随机过程特征及贝叶斯公式
1.2 似然概率
1.3 贝叶斯滤波公式：预测+更新
2、卡尔曼滤波
2.1 KF的 2 个假设
2.2 KF 的 5 个公式：预测 + 更新
2.3 KF 5 个公式的矩阵形式
3、卡尔曼滤波 MATLAB 代码实现与调参
3.1 黑盒系统模型的设计与 KF 算法实现
3.2 Q、R 参数影响及 X0、P0 初始值影响
3.3 精确系统模型下的 KF 效果
4、粒子滤波
1、贝叶斯滤波
1.1 随机过程特征及贝叶斯公式
随机过程
续
离散随机变量贝叶斯公式
连续随机变量下的贝叶斯公式
1.2 似然概率
似然模型
正态分布
狄拉克函数
1.3 贝叶斯滤波公式：预测+更新
状态方程、观测方程
预测步
更新步
贝叶斯滤波缺陷
2、卡尔曼滤波
2.1 KF的 2 个假设
2.2 KF 的 5 个公式：预测 + 更新
方差影响
2.3 KF 5 个公式的矩阵形式
3、卡尔曼滤波 MATLAB 代码实现与调参
3.1 黑盒系统模型的设计与 KF 算法实现
（1）测量信号生成：设计一个抛物线形状不带噪声的真实信号，传感器测量值带有白噪声，期望为 0，方差为 0.01
%% 生成一段带有白噪声的传感器测量信号
t = 0.1:0.01:1;
L = length(t);
x = zeros(1,L); % 真实信号
y = zeros(1,L); % 测量信号
for i=1:L
x(i) = t(i)^2; % 真实信号（无噪声）
y(i) = x(i)+normrnd(0,0.1); % 测量信号（增加白噪声：期望为 0，标准差为 0.1）
end
plot(t,x,t,y,'LineWidth',2);
legend('信号真实值（无噪）','传感器测量值（方差0.1^2）');查看：
带白噪音的信号
（2） 系统模型设计：盲猜模型为一条斜率为 0 的直线（完全不准确），具体体现在状态方程中就是状态转移矩阵 F = 1（本例先采用一维状态变量），也即下一时刻的系统状态估计假设完全不变，唯一导致变化的就是来自于噪声信号 Q 的干扰。显然和实际情况（抛物线式增长）有较大的差距，我们需要观察卡尔曼滤波器如何处理这种情况。
接下来设定 Q、R 参数，分别为过程噪声的方差和测量噪声的方差。由于在实际中我们对过程噪声和测量噪声的情况完全未知，只了解是一个均值为零的白噪声，所以需要对这两个噪声的方差进行大致的猜测，猜测的值越小，表示越信任该结果。如果假设 Q 为 0，也就意味着完全信任系统预测值，也就不需要测量值进行更新矫正了，所以 Q 和 R 不能设置为 0。这里随机分别设置为 1
最后还需要给出初始时刻的期望值和方差值，假定分别为：0.01、0.01^2。
%% 系统模型设计
% X(K) = X(K-1)+Q
% Y(K) = X(K)+R
% 令 Q~(0,1)，R~(0,1)
F1 = 1; % 状态方程的矩阵设为 1，即假状态在下一时刻不变，只有噪声 Q 影响
H1 = 1; % 观测方程的矩阵，也设为 1
Q1 = 1; % 过程噪声的方差设定为 1
R1 = 1; % 测量噪声的方差设定为 1
Xplus1 = zeros(1,L); % 存放后验期望值
Xplus1(1) = 0.01; % 假设期望初值为 0.01，XPlus1~(0.01,0.01)
Pplus1 = 0.01^2; % 假设方差初值为 0.01（3）卡尔曼滤波器的 5 个公式实现：
%% 滤波算法
% 卡尔曼滤波算法 5 个公式
% X(K)minus = F*X(K-1)plus;
% P(K)minus = F*P(K-1)plus*F'+Q
% K=P(K)minus*H'inv(H*P(K)minus*H'+R)
% X(K)plus=X(K)minus+K*(y(k)-H*X(K)minus)
% P(K)plus=(I-K*H)*P(K)minus
for i=2:L
% 预测步
Xminus1=F1*Xplus1(i-1); % 这里没有用数组存储，而是直接使用了一维变量
Pminus1=F1*Pplus1*F1'+Q1;
% 更新步
K1=(Pminus1*H1')*inv(H1*Pminus1*H1'+R1);
Xplus1(i)=Xminus1+K1*(y(i)-H1*Xminus1); % 这里使用了数组进行存储，因为需要记录显示结果
Pplus1=(1-K1*H1)*Pminus1;
end
plot(t,x,'r',t,y,'g',t,Xplus1,'b','LineWidth',2);
legend('信号真实值','传感器测量值（方差0.1^2）','KF 滤波估算值(Q=0.01 R=1)');查看卡尔曼滤波的效果，可以看到预测值大致跟随信号实际值，但方差很大，抖动剧烈，在实际工况中抖动较大的信号基本无法使用，会带来磨损严重等后果，但大体上已经实现了一些滤波效果
粗糙模型下卡尔曼滤波效果
3.2 Q、R 参数影响及 X0、P0 初始值影响
（1）Q、R 参数影响及调试：系统过程噪声假设的方差 Q =1 值很大，导致滤波器的估计值更接近于测量结果，而非模型预测的结果，所以结果大致跟随测量噪声，波动较大，但仍然可以看出整体的方差有所减小。现在将 Q 值减小为 0.01，测量噪声方差 R 保持不变，使系统预测值的信任比重增大，效果如下：
卡尔曼滤波器，减小Q
可以明显看到减小 Q 值后的滤波结果形状（蓝色）更接近于预测值的形状，方差也大幅减小，但由于系统模型与实际模型差距较大，导致估计值与实际值仍然有较大的偏差，但不可否认合理的卡尔曼滤波器参数会大幅降低噪声的频率。
（2）状态初始值影响：首先将状态量初值由 0.01 增大至 1，观察到只在初始时刻影响比较大，但由于卡尔曼滤波器的接入，使得下一时刻的估计值很快逼近回到测量值附近，所以可以得出结论：期望值的初值的选取对滤波结果影响较小。
状态初始值影响
（3）方差初始值影响：由于状态量方差本身就是过程噪声方差 Q，可以直接给出初始值为 Q 即 1，但由于前面的结果都是在方差初始值为 0.01^2 的条件下测试的，而滤波效果也没有太大的偏差，所以初始方差的选择也对滤波结果无影响，为了对比，也给出方差初始值为 1 的滤波效果：
方差初始值影响
3.3 精确系统模型下的 KF 效果
现在给出一个更加符合信号真实值（抛物线轨迹）的精确模型
%% 模型 2
% X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*（1、2!)+Q
% Y(K) = H*X(K)+R，H=[1 0 0] 观测方程
dt=t(2)-t(1);
F2=[1,dt,0.5*dt^2;0,1,dt;0,0,1];
H2=[1,0,0];
Q2=diag([1, 0.01, 0.0001]);
R2=20;
% 设置初值
Xplus2=zeros(3,L);
Xplus2(1,1)=0.1^2;
Xplus(2,1)=0;
Xplus(3,1)=0;
Pplus2=diag([0.01,0.01,0.0001]);
for i=2:L
Xminus2=F2*Xplus2(:,i-1);
Pminus2=F2*Pplus2*F2'+Q2;
%更新步
K2=(Pminus2*H2')*inv(H2*Pminus2*H2'+R2);
Xplus2(:,i)=Xminus2+K2*(y(i)-H2*Xminus2);
Pplus2=(eye(3)-K2*H2)*Pminus2;
end
plot(t,x,'r',t,y,'g',t,Xplus1,'b',t,Xplus2(1,:),'m','LineWidth',2);滤波效果如下：可以看出采用更为精确的模型可以提高预测的精度，使预测值更加接近信号真实值。但卡尔曼滤波器仍然很难将噪音完全消除。
精确模型KF 对比 粗糙模型KF
卡尔曼滤波的优点在于可以借助于精确的系统状态方程，进行递推式在线滤波，除了卡尔曼滤波器外，常用的方法还包括频域分析中的低通滤波器，如：设计巴特沃斯低通滤波器对高频噪声进行离线滤波。