KF、EKF、UKF的matlab代码实现

概述

本文分享卡尔曼滤波、扩展卡尔曼滤波和无迹卡尔曼滤波的matlab代码实现。

正文

卡尔曼滤波：

%{
X(k) = F*X(k-1) + Q %预测方程
Y(k) = H*X(k) + R   %观测方程
%}
%生成一段时间t
t=0.1:0.01:1;%从0.1s开始采样，每0.01s采样一次，到1s为止
L=length(t);%总共采了length(t)次，赋给L
%生成真实信号x，以及观测信号y
%首先初始化
x=zeros(1,L);%生成1行L列的数组
y=x;
%生成真实信号波形，设x=t^2;
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);
%滤波算法
%预测方程：
%先建模：
%{
%模型一：因为看观测数据看不出规律，所以先建立一个粗糙的模型:f(x)=x
%那么X(k)=X(k-1)+Q Q~N(0,1):方差先自己给定
%观测方程：Y(k)=X(k)+R R~N(0,1):测量噪声协方差R服从正态分布，期望为0，
%                                         方差为1（自己先给定的数值）
F1=1;%由于建模模型为1维模型，所以矩阵是一维矩阵
H1=1;
Q1=1;%越小越相信预测值
R1=1;%越小越相信观测值
%初始化x(k)+：因为卡尔曼是递推算法，所以需要存储过程量
Xplus1=zeros(1,L);
%设置一个初值，假设Xplus1(1)~N(0.01,0.01^2):设X(第一个值)= 0.01，方差小一点说
%明很相信初始值
Xplus1(1)=1;%初始值只对前期收敛有影响
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*K(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,t,y,t,Xplus1,'LineWidth',2);
legend('x','y','Xplus1');
%}
%模型二：对x进行泰勒级数展开
%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 + R2 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
%那么F=1 dt 0.5*dt^2
%      0 1  dt 
%      0 0  1
%H=[1 0 0]
%Q=[ Q2 0 0    (协方差0表示两个元素之间不相关)
%    0 Q3 0
%    0 0 Q4  (都是自己初定的1)
dt = t(2) - t(1);
F2 = [1, dt, 0.5*dt^2;0, 1, dt;0, 0, 1];
H2 = [1, 0, 0];
Q2 = [0.1, 0, 0;0, 0.01, 0;0, 0, 0.001];
R2 = 1;%因为观测只有一个，所以观测协方差矩阵是一维
%初始化
Xplus2=zeros(3,L);
Xplus2(1,1)=0.01^2;
Xplus2(2,1)=0;
Xplus2(3,1)=0;
Pplus2=[0.01, 0, 0;0, 0.01, 0;0, 0, 0.01];
for i=2:L
    Xminus2=F2*Xplus2(:,i-1);%把Xplus2的(i-1)列的向量与F2相乘
    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,t,y,t,Xplus2(1,:),'LineWidth',2);
legend('x','y','Xplus2');

滤波效果：

扩展卡尔曼滤波：

%设模型方程：f(x) = sin(3*x)
%观测方程：y = x^2;
%注意似然概率是多峰分布，具有强烈的非线性(EKF缺陷)：当y=4时，不知道x=2还是-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.3);
end
%EKF
Xplus=zeros(1,n);
%初始化
Pplus=0.1;
Xplus(1)=0.1;
Q=0.0001;
R=1;
for i=2:n
    %预测步
    A=3*cos(3*Xplus(i-1));%雅各比矩阵
    Xminus=sin(3*Xplus(i-1));
    Pminus=A*Pplus*A'+Q;
    %更新步
    C=2*Xminus;%雅各比矩阵
    K=Pminus*C*inv(C*Pminus*C'+R);
    Xplus(i)=Xminus+K*(y(i)-Xminus^2);
    Pplus=(eye(1)-K*C)*Pminus;
end
plot(t,x,'r',t,y,'g',t,Xplus,'m','LineWidth', 2);
legend('x','y','Xplus');    

滤波效果：

无迹卡尔曼滤波：

%UKF算法
t=0.01:0.01:1;
%生成真实的x与观测z
x=zeros(2,100);
z=zeros(2,100);
x(1,1)=0.2;
x(2,1)=0.2;
%随便写一个非线性xk=f(xk-1) zk=h(xk)+R
for i=2:100
    x(1,i)=sin(x(1,i-1))+2*cos(x(2,i-1));
    x(2,i)=3*cos(x(1,i-1))+sin(x(2,i-1));
    z(1,i)=x(1,i)+x(2,i)^3+normrnd(0,1);
    z(2,i)=x(1,i)^3+x(2,i)+normrnd(0,1);
end
%设初值
X=zeros(2,100);
X(1,1)=0.1;
X(2,1)=0.2;
Pplus=eye(2);
Q=eye(2);
R=eye(2);
%设w(1,2n+1)
n=2;%n代表X的维数
w=zeros(n,2*n+1);
lamda = 2;
for i=1:2*n+1
    w(i)=1/(2*(n+lamda));
end
w(1)=lamda/(n+lamda);
%UKF算法实体
for i=2:100
    %拆x_sigma
    xsigma = zeros(n,2*n+1);
    L=chol(Pplus);%对正定矩阵进行分解
    xsigma(:,1)=X(:,i-1);
    for j=1:n
        xsigma(:,j+1)=xsigma(:,1)+sqrt(n+lamda)*L(:,j);
        xsigma(:,j+1+n)=xsigma(:,1)-sqrt(n+lamda)*L(:,j);
    end
    %预测步,生成xsigma_minus
    xsigmaminus=zeros(n,2*n+1);
    for j=1:2*n+1
        %xk=f(xk-1)
        xsigmaminus(1,j)=sin(xsigma(1,j))+3*cos(xsigma(2,j));
        xsigmaminus(2,j)=3*cos(xsigma(1,j))+sin(xsigma(2,j));
    end
    %求期望协方差矩阵
    xhatminus=zeros(n,1);
    Pminus=zeros(n,n);
    for j=1:2*n+1
        xhatminus=xhatminus+w(j)*xsigmaminus(:,j);
    end
    for j=1:2*n+1
        Pminus=Pminus+w(j)*(xsigmaminus(:,j)-xhatminus)*(xsigmaminus(:,j)-xhatminus)';
    end
    Pminus=Pminus+Q;
    %更新步
    %再拆sigma点
    xsigma=zeros(n,2*n+1);
    xsigma(:,1)=xhatminus;
    L1=chol(Pminus);
    for j=1:n
        xsigma(:,j+1)=xsigma(:,1)+sqrt(n+lamda)*L1(:,j);
        xsigma(:,j+1+n)=xsigma(:,1)-sqrt(n+lamda)*L1(:,j);
    end
    %生成y,yhat
    yhat=zeros(n,1);
    for j=1:2*n+1
        y(1,j)=xsigma(1,j)+xsigma(2,j)^3;
        y(2,j)=xsigma(1,j)^3+xsigma(2,j);
        yhat=yhat+w(j)*y(:,j);
    end
    %求Py Pxy
    Py=zeros(n,n);
    Pxy=zeros(n,n);
    for j=1:2*n+1
       Pxy=Pxy+w(j)*(xsigma(:,j)-xhatminus)*(y(:,j)-yhat)';
       Py=Py+w(j)*(y(:,j)-yhat)*(y(:,j)-yhat)';
    end
    Py=Py+R;
    %求卡尔曼增益
    K=Pxy*inv(Py);
    %观测数据
    Y=zeros(n,1);
    Y(1,1)=z(1,i);
    Y(2,1)=z(2,i);
    %更新
    X(:,i)=xhatminus+K*(Y-yhat);
    Pplus=Pminus+K*Py*K';
end
plot(t,x(1,:),'r',t,X(1,:),'b','LineWidth',3);

滤波效果：

总结

通过上述实验，可以看出。系统的状态空间方程越精细，卡尔曼滤波效果越好。其次，扩展卡尔曼对于处理高度线性化的系统，滤波效果并不理想。最后，无迹卡尔曼模型最为复杂，参数调节也最为复杂，对高度线性化系统的滤波效果比扩展卡尔曼滤波稍好。