扩展卡尔曼滤波

扩展卡尔曼滤波

特点：可以处理非线性问题，速度快，不稳定，遇到强非线性会发散。
将非线性线性化的卡尔曼滤波

一、适用条件

最好处理弱非线性
已知m个传感器，观测同一个量，生成附带噪声的m组观测数据。
m个传感器等间隔（dt已知），同时观测（已知观测时刻），观测数据长度L。
得到y1[L]…ym[L]共m个长度为L的数组。
观测数据共L个Y（m1），其中Y的第i行为yi
估算模型（一般用n阶系统）
F（nn）：根据导数模型可以构造F矩阵。
H（mn）：第一列为1，其余列为0。
Q（nn）：协方差矩阵，一般只有主对角线有值，低阶方差大，高阶小。
R（mm）：协方差矩阵，一般只有主对角线有值，一般较大，观测不可信。
A（nn）：状态方程F（n1）右侧的雅可比矩阵
C（mn）：观测方程H（m1）右侧的雅可比矩阵
Xplus（n1）给初值时影响不大
Pplus（nn）给初值时候可以小一些，认为初值精准
具体公式：小括号里指第几步的意思
A（nn）=状态方程（n1）右侧的雅可比矩阵（代入Xplus(i-1)）
Xminus（n1）=状态方程（n1）（代入Xplus(i-1)）
Pminus（nn）=A（nn）Pplus（n1）A’（nn）+Q(nn)（代入Xplus(i-1)，A用刚刚算出的A即A(i)）
C（mn）=观测方程（m1）右侧的雅可比矩阵（代入Xminus(i)）
K（nm）=（Pminus（nn）C’（mn））/（C（mn）Pminus（nn）C’（mn）+R（mm））（逆矩阵的简写）（代入Xminus(i)，C用刚刚算出的C即C(i)）
Xplus（n1）=Xminus（n1）+K（nm）（Y（m1）-H（m1））（H中代入刚算出的Xminus(i)）
Pplus（nn）=（单位阵（nn）-K（nm）C（mn））Pminus（nn）

二、代码

clear
clc
%x(k)=sin(3*x(k-1))
%y(k)=x(k)^2
t=0.01:0.01:1;
n=length(t);
x=zeros(1,n);
y=zeros(1,n);
x(1)=0.1;
y(1)=0.1^2;
for i=2:n
    x(i)=sin(3*x(i-1));
    y(i)=x(i)^2+normrnd(0,0.7);
end
plot(t,x,t,y,'LineWidth',2);
Q=0.1;
R=1;
Xplus=zeros(1,n);
Pplus=zeros(1,n);
Xminus=zeros(1,n);
Pminus=zeros(1,n);
K=zeros(1,n);
A=zeros(1,n);
C=zeros(1,n);
Xplus(1)=0.1;%期望 初值即使是1也无妨
Pplus(1)=0.1;%方差
for i=2:n
    A(i)=3*cos(3*Xplus(i-1));
    Xminus(i)=sin(3*Xplus(i-1));
    Pminus(i)=A(i)*Pplus(i-1)*A(i)'+Q;
    C(i)=2*Xminus(i);
    K(i)=Pminus(i)*C(i)'/(C(i)*Pminus(i)*C(i)'+R);
    Xplus(i)=Xminus(i)+K(i)*(y(i)-Xminus(i)^2);
    Pplus(i)=(eye(1)-K(i)*C(i))*Pminus(i);
end
plot(t,x,t,Xplus,'LineWidth',2);