%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

%%%%貝葉斯濾波與卡爾曼濾波第八講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