卡尔曼滤波

卡尔曼滤波

---

适用条件：

只能处理线性函数（非线性要用扩展卡尔曼或粒子）
已知m个传感器，观测同一个量，生成附带噪声的m组观测数据。
m个传感器等间隔（dt已知），同时观测（已知观测时刻），观测数据长度L。
得到y1[L]…ym[L]共m个长度为L的数组。
观测数据共L个Y（m1），其中Y的第i行为yi
估算模型（一般用n-1阶导数模型，即展开有n项）
F（nn）：根据导数模型可以构造F矩阵。
H（mn）：第一列为1，其余列为0。
Q（nn）：协方差矩阵，一般只有主对角线有值，低阶方差大，高阶小。
R（mm）：协方差矩阵，一般只有主对角线有值，一般较大，观测不可信。
K（nm）：参数矩阵。
Xplus（n1）给初值时导数初值一般为0
Pplus（nn）给初值时候可以小一些，认为初值精准
预测
Xminus（n1）=F（nn）Xplus（n1）
Pminus（nn）=F（nn）Pplus（nn）F’（nn）+Q(nn)
更新
K（nm）=（Pminus（nn）H’（mn））/（H（mn）Pminus（nn）H’（mn）+R（mm））（逆矩阵的简写）
Xplus（n1）=Xminus（n1）+K（nm）（Y（m1）-H（mn）Xminus（n1））
Pplus（nn）=（单位阵（nn）-K（nm）H（mn））Pminus（nn）
最终结果为Xplus的第一行
ps：在这次计算时，预测步的右侧均为上一次结果，更新均为这一次结果。

代码实现：

%卡尔曼滤波
clear
clc
%X(K)=F*X(K-1)+Q 线性模型
%Y(K)=H*X(K)+R 线性模型
%生成时间t
t=0.1:0.01:1;
L=length(t);%时刻数量
x=zeros(1,L);
y=zeros(1,L);
y1=zeros(1,L);%多传感器融合
y2=zeros(1,L);%多传感器融合
for i=1:L
    x(i)=t(i)^2;%生成真实信号
    y(i)=x(i)+normrnd(0,0.1);%生成观测 附带噪声（期望，标准差）
    y1(i)=x(i)+normrnd(0,0.1);
    y2(i)=x(i)+normrnd(0,0.1);
end
%在后续只能使用观测序列y 得到结果与x做对比
%plot(t,x,t,y,'LineWidth',2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模型一%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%X(K)=X(K-1)+Q1 粗糙建模
%Y(K)=X(K)+R1
%Q1~N(0,1)可多次修改
%R1~N(0,1)可多次修改
F1=1;
H1=1;
Q1=1;
R1=1;
%预分配
Xplus1=zeros(1,L);
Pplus1=zeros(1,L);
Xminus1=zeros(1,L);
Pminus1=zeros(1,L);
K1=zeros(1,L);
%初值不影响最终结果
Xplus1(1)=0.01;%期望
Pplus1(1)=0.01^2;%方差 给的小一点 对初值自信
for i=2:L
    Xminus1(i)=F1*Xplus1(i-1);
    Pminus1(i)=F1*Pplus1(i-1)*F1'+Q1;
    K1(i)=(Pminus1(i)*H1')/(H1*Pminus1(i)*H1'+R1);
    Xplus1(i)=Xminus1(i)+K1(i)*(y(i)-H1*Xminus1(i));
    Pplus1(i)=(1-K1(i)*H1)*Pminus1(i);
end
%Xplus1为最终结果
%plot(t,x,t,y,t,Xplus1,'LineWidth',2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%模型二%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%X(K)=X(K-1)+X'(K-1)*dt+0.5*X''(K-1)*dt*dt+Q2
%Y(K)=X(K)+R2
%状态变量X=[X(k) X'(k) X''(k)]'列向量
%Y(K)=H*X+R2 H=[1,0,0]%%R2是数
%预测方程
%X'(K)=X'(K-1)+X''(K-1)*dt+Q3
%X''(K)=X''(K-1)+Q4
%F=1 dt 0.5*dt*dt
%  0  1     dt
%  0  0     1
%Q=Q2  0   0
%  0  Q3   0
%  0  0   Q4协方差矩阵 认为噪声之间不相关 对二阶导数建模自信（来自猜测）
dt=(t(L)-t(1))/(L-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=20;%观测认为不可信
Xplus2=zeros(3,L);%预分配
Pplus2=zeros(3,3,L);
Xminus2=zeros(3,L);
Pminus2=zeros(3,3,L);
K2=zeros(3,L);
Xplus2(1,1)=0.1^2;%初值
Xplus2(2,1)=0;
Xplus2(3,1)=0;
Pplus2(:,:,1)=[0.01,0,0;0,0.01,0;0,0,0.0001];
for i=2:L
    Xminus2(:,i)=F2*Xplus2(:,i-1);
    Pminus2(:,:,i)=F2*Pplus2(:,:,i-1)*F2'+Q2;
    K2(:,i)=(Pminus2(:,:,i)*H2')/(H2*Pminus2(:,:,i)*H2'+R2);
    Xplus2(:,i)=Xminus2(:,i)+K2(:,i)*(y(i)-H2*Xminus2(:,i));
    Pplus2(:,:,i)=(eye(3)-K2(:,i)*H2)*Pminus2(:,:,i);
end
%plot(t,x,t,y,t,Xplus2(1,:),'LineWidth',2);
%----------------------------当有两个传感器同时测量同一个数据-----------------------------
%Q方阵维数同x维数 R矩阵同y维数
Q3=[1,0,0;0,0.01,0;0,0,0.0001];
R3=[3,0;0,3];%观测认为不可信
F3=[1 dt 0.5*dt^2;0 1 dt;0 0 1];%沿用三阶泰勒展开模型
H3=[1,0,0;1,0,0];
%预分配
Xplus3=zeros(3,L);
Pplus3=zeros(3,3,L);
Xminus3=zeros(3,L);
Pminus3=zeros(3,3,L);
K3=zeros(3,2,L);
%初始化
Xplus3(1,1)=0.1^2;
Xplus3(2,1)=0;
Xplus3(3,1)=0;
Pplus3(:,:,1)=[0.01,0,0;0,0.01,0;0,0,0.0001];
Y=[y1;y2];
for i=2:L
    %预测
    Xminus3(:,i)=F3*Xplus3(:,i-1);
    Pminus3(:,:,i)=F3*Pplus3(:,:,i-1)*F3'+Q3;
    %更新
    K3(:,:,i)=(Pminus3(:,:,i)*H3')/(H3*Pminus3(:,:,i)*H3'+R3);
    Xplus3(:,i)=Xminus3(:,i)+K3(:,:,i)*(Y(:,i)-H3*Xminus3(:,i));
    Pplus3(:,:,i)=(eye(3)-K3(:,:,i)*H3)*Pminus3(:,:,i);
end
plot(t,x,t,Xplus3(1,:),'LineWidth',2);