贝叶斯滤波与卡尔曼滤波第八讲代码

%https://www.bilibili.com/read/cv5562164
%%%kalman filter
%多看别人源代码
%多练
%生成一段时间t
t = 0.1:0.01:1;
L = length(t);
%生成真实信号x，以及观测y
x = zeros(1,L);
y = x;

%生成信号，设x=t^2
for i = 1:L
    x(i)=t(i)^2;
    y(i)=x(i)+normrnd(0,0.1);%正态分布
end

% plot(t,x,t,y,'LineWidth',2)
%滤波算法
% plot(t,y)
%预测方程不好写
F1 = 1;
H1 = 1;
Q1 = 1;
R1 = 1;
%1 mean model one
Xplus1 = zeros(1,L);
%initial value
Xplus1(1) = 1;
Pplus1=0.01^2;

%+ 更新 后验概率
%- 预测 先验概率 x0- x1- x2+
%卡尔曼滤波就是均值和方差推来推去
for i=2:L
    %预测步
    Xminus1 = F1*Xplus1(i-1);
    Pminus1 = F1*Pplus1*F1'+Q1;
    %UPDATA
    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)

%模型2

%%%kalman filter
%多看别人源代码
%多练
%生成一段时间t
t = 0.1:0.01:1;
L = length(t);
%生成真实信号x，以及观测y
x = zeros(1,L);
y = x;

%生成信号，设x=t^2
for i = 1:L
    x(i)=t(i)^2;
    y(i)=x(i)+normrnd(0,1);%正态分布
    y2(i)=x(i)+normrnd(0,1);
end

% plot(t,x,t,y,'LineWidth',2)
%滤波算法
% plot(t,y)
%预测方程不好写
F1 = 1;
H1 = 1;
Q1 = 1;
R1 = 1;
%1 mean model one
Xplus1 = zeros(1,L);
%initial value
Xplus1(1) = 1;
Pplus1=0.01^2;

%+ 更新 后验概率
%- 预测 先验概率 x0- x1- x2+
%卡尔曼滤波就是均值和方差推来推去
for i=2:L
    %预测步
    Xminus1 = F1*Xplus1(i-1);
    Pminus1 = F1*Pplus1*F1'+Q1;
    %UPDATE
    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)

%模型2
dt = t(2) - t(1);
F2 = [1,dt,0.5*dt^2;
      0,1,dt;
      0,0,1];
  
H2 = [1,0,0];
% Q = Q2 0 0
%     0  Q3 0
%     0  0  q4
% 协方差矩阵
Q2 = [1,0,0;
      0,0.01,0;
      0,0,0.0001];

R2 = 20;
%设置初值
Xplus2 = zeros(3,L);
Xplus2(1,1) = 0.1^2;
Xplus2(2,1) = 0;
Xplus2(3,1) = 0;

Pplus2 = [0.01,0,0;
          0,0.01,0;
          0,0,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,:),'k','LineWidth',2)
%  Q R方阵
%  X Y 列向量

% 问题2，两个传感器，进行滤波
% 传感器融合，主从雷达
% Y1(K)=X(K)+R

% Y2(K)=X(K)+R

%H=[1 1]T (列向量) X=X(K)

%H=1   0     0     X=X(K)   X'(K)   X''(K)
F3 = [1,dt,0.5*dt^2;
      0,1,dt;
      0,0,1];

H3 = [1,0,0;
      1,0,0];
Q3 = [1,0,0;
      0,0.01,0;
      0,0,0.0001];
  
R3 = [3, 0;
      0, 3];%%%%%一定要注意是协方差矩阵

 %%%设置初值%%%%

Xplus3 = zeros(3,L);
Xplus3(1,1) = 0.1^2;
Xplus3(2,1) = 0;
Xplus3(3,1) = 0;

Pplus3 = [0.01,0,0;
          0,0.01,0;
          0,0,0.0001];
      
for i = 2:L
%    predict
    Xminus3 = F3 * Xplus3(:,i-1);
    Pminus3 = F3 * Pplus3 * F3' + Q3;
    
%     update
    K3 = (Pminus3 * H3') * inv(H3 * Pminus3 * H3' + R3);% 3*2
    Y = zeros(2,1);
    Y(1,1) = y(i);
    Y(2,1) = y2(i);
    
    Xplus3(:,i) = Xminus3 + K3*(Y - H3*Xminus3);
    Pplus3 = (eye(3) - K3 * H3 )*Pminus3;
end


plot(t,x,'r',t,y,'g',t,Xplus2(1,:),'k',t,Xplus3(1,:),'m','LineWidth',2)

%%%%贝叶斯滤波与卡尔曼滤波第八讲matlab代码%%%%%%

%%%%kalman filter

%X(K)=F*X(K-1)+Q

%Y(K)=H*X(K)+R

%%%第一个问题，生成一段随机信号，并滤波



%生成一段时间t

t=0.1:0.01:1;

L=length(t);

%生成真实信号x，以及观测y

%首先初始化

x=zeros(1,L);

y=x;

y2=x;

%生成信号，设x=t^2

for i=1:L

    x(i)=t(i)^2;

    y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差

    y2(i)=x(i)+normrnd(0,0.1);

end

%%%%%%%%%%%信号生成完毕%%%%



%%%%%%%滤波算法%%%%%%

%%%预测方程观测方程怎么写%%%

%观测方程好写Y(K)=X(K)+R R~N(0,1)

%预测方程不好写%%%%，在这里，可以猜一猜是线性增长，但是大多数问题，信号是杂乱无章的，怎么办

%模型一，最粗糙的建模

%X(K)=X(K-1)+Q

%Y(K)=X(K)+R

%猜Q~N(0,1);

F1=1;

H1=1;

Q1=1;

R1=1;

%初始化x(k)+

Xplus1=zeros(1,L);%plus + 的英语 minus -的英语

%我们会经常用到Xplus,Xminus,Pplus,Pminus



%设置一个初值，假设Xplus1(1)~N(0.01,0.01^2)

Xplus1(1)=0.01;

Pplus1=0.01^2;

%%%卡尔曼滤波算法

%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

%模型2

%X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2

%Y(K)=X(K)+R R~N(0,1)

%此时状态变量X=[X(K)    X'(K)    X''(K) ]T(列向量）

%Y(K)=H*X+R    H=[1   0   0](行向量）

%预测方程

%X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2

%X'(K)=0*X(K-1)+X'(K-1)+X''(K-1)*dt+Q3

%X''(K)=0*X(K-1)+0*X'(K-1)+X''(K-1)+Q4  %%多项式信号多求几阶导数，总会比较平缓，而

%X''(K)=X''(K-1)+Q3正是描述平缓的随机过程，这种建模相对精细一些，适用范围也较广

%F= 1    dt     0.5*dt^2

%      0     1         dt

%      0      0         1

%H=[1     0     0]

%Q= Q2   0    0

%      0     Q3   0

%       0     0     Q4 协方差矩阵

dt=t(2)-t(1);

F2=[1,   dt,   0.5*dt^2;0,   1,   dt;0,   0,     1];%%%此处要注意矩阵是否病态，若dt特别小，易导致矩阵病态或精度丢失

H2=[1,0,0];

Q2=[1,  0,  0;0,  0.01,  0;0,  0,  0.0001];

R2=3;

%%%设置初值%%%%

Xplus2=zeros(3,L);

Xplus2(1,1)=0.1^2;

Xplus2(2,1)=0;

Xplus2(3,1)=0;

Pplus2=[0.01, 0,   0;0,    0.01,   0;0,   0,    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



%%%可以进行在线滤波，实时滤波%%%%



%问题2，两个传感器，进行滤波

% Y1(K)=X(K)+R

% Y2(K)=X(K)+R

%H=[1 1]T (列向量) X=X(K)

%H=1   0     0     X=X(K)   X'(K)   X''(K)

%     1   0     0

    

F3=[1,   dt,   0.5*dt^2;0,   1,   dt;0,   0,     1];%%%此处要注意矩阵是否病态，若dt特别小，易导致矩阵病态或精度丢失

H3=[1,0,0;1,0,0];

Q3=[1,  0,  0;0,  0.01,  0;0,  0,  0.0001];

R3=[3,0;0,3];%%%%%一定要注意是协方差矩阵

%%%设置初值%%%%

Xplus3=zeros(3,L);

Xplus3(1,1)=0.1^2;

Xplus3(2,1)=0;

Xplus3(3,1)=0;

Pplus3=[0.01, 0,   0;0,    0.01,   0;0,   0,    0.0001];

for i=2:L

    %%%预测步%%%

    Xminus3=F3*Xplus3(:,i-1);

    Pminus3=F3*Pplus3*F3'+Q3;

    %%%更新步%%%%

    K3=(Pminus3*H3')*inv(H3*Pminus3*H3'+R3);

    Y=zeros(2,1);

    Y(1,1)= y(i);

    Y(2,1)=y2(i);

    Xplus3(:,i)=Xminus3+K3*(Y-H3*Xminus3);

    Pplus3=(eye(3)-K3*H3)*Pminus3;

end



plot(t,x,'r',t,y,'g',t,Xplus2(1,:),'k',t,Xplus3(1,:),'m','LineWidth',2);
作者：忠厚老实的老王
https://www.bilibili.com/read/cv5562164
出处： bilibili