ekf.m

扩展卡尔曼滤波器

%对一个控制系统:状态方程:
    %Ed(i+1)=t*(Ed(i)+Rs(i)*Eq(i)+x(i)*Ed(i)+Wr(i)*Eq(i)+T(i)*Rs(i))+Ed(i)+w(i);
    %Eq(i+1)=t*(Eq(i)+x(i)*Eq(i)+Rs(i)*Ed(i)+Wr(i)*Ed(i)+T(i)*x(i))+Eq(i)*T(i)+w(i);
    %Wr(i+1)=t*(Wr(i)+Rs(i)*Wr(i)+x(i)*Ed(i)+x(i)*Eq(i)+Wr(i)*Ed(i))+Wr(i)+w(i);
    %Rs(i+1)=Rs(i)+w(i);
    %x(i+1)=x(i)+w(i);
    %T(i+1)=T(i)+w(i);
%测量方程
    %ID(i)=Ed(i)+v(i);  相当于书上的z
    %IQ(i)=Eq(i)+v(i);   z的协方差
%w(i),v(i)为高斯白噪声
%下面我先构造一组测量数据,然后根据测量数据估计状态变量的值%
%构造测量数据%
t=0.01;
Ed(1)=0;
Eq(1)=0;
Wr(1)=2;
Rs(1)=0.02;
x(1)=0.4;
T(1)=1;
Q=1e-10;
R=1e-10;
n=100;
m=6;
p=2;
w=random('norm',0,Q,m,n);
v=random('norm',0,R,p,n);
x_true=zeros(6,n+1);%构造6*（n+1）的零矩阵
y_true=zeros(2,n+1);%构造2*（n+1）的零矩阵
x_true(:,1)=[Ed(1);Eq(1);Wr(1);Rs(1);x(1);T(1)];
for i=1:n
    Ed(i+1)=t*(Ed(i)+Rs(i)*Eq(i)+x(i)*Ed(i)+Wr(i)*Eq(i)+T(i)*Rs(i))+Ed(i)+w(1,i);
    Eq(i+1)=t*(Eq(i)+x(i)*Eq(i)+Rs(i)*Ed(i)+Wr(i)*Ed(i)+T(i)*x(i))+Eq(i)*T(i)+w(2,i);
    Wr(i+1)=t*(Wr(i)+Rs(i)*Wr(i)+x(i)*Ed(i)+x(i)*Eq(i)+Wr(i)*Ed(i))+Wr(i)+w(3,i);
    Rs(i+1)=Rs(i)+w(4,i);
    x(i+1)=x(i)+w(5,i);
    T(i+1)=T(i)+w(6,i);
    ID(i)=Ed(i)+v(1,i);
    IQ(i)=Eq(i)+v(2,i);
    x_true(:,i+1)=[Ed(i+1);Eq(i+1);Wr(i+1);Rs(i+1);x(i+1);T(i+1)];
    y_true(:,i)=[ID(i);IQ(i)];
end
% HZB=0.00:0.01:1.00;
% plot(HZB,x)
%估计状态变量的值%
syms Ed Eq Wr Rs x T;
t=0.01;
F=[t*(Ed+Rs*Eq+x*Ed+Wr*Eq+T*Rs)+Ed t*(Eq+x*Eq+Rs*Ed+Wr*Ed+T*x)+Eq t*(Wr+Rs*Wr+x*Ed+x*Eq+Wr*Ed)+Wr Rs x T];
h=[Ed Eq];
X=[Ed Eq Wr Rs x T];
FF=jacobian(F,X);
hh=jacobian(h,X);
N=100;
HZB=0.00:0.01:1;
X1=[0 0 2 0.0154 0.3787 0.99]';
Po=0.0001*eye(6); %eye(6)定义了一个6*6的矩阵，对角线元素为1，别的为0
Z=[ID' IQ'];
Q=1e-10;
R=1e-10;
x_filter=zeros(6,N+1);
cova_filter=zeros(6,N+1);
x_filter(:,1)=[0 0 2 0.0154 0.3787 0.99]';
cova_filter(:,1)=diag(Po);
for i=1:N
    X=X1';
    Ed=X(1);
    Eq=X(2);
    Wr=X(3);
    Rs=X(4);
    x=X(5);
    T=X(6);
    P=eval(FF)*Po*(eval(FF))'+Q*eye(6);
    X=eval(F);
    Ed=X(1);
    Eq=X(2);
    Wr=X(3);
    Rs=X(4);
    x=X(5);
    T=X(6);
    K=P*eval(hh)'*inv(eval(hh)*P*eval(hh)'+R);
    P1=(eye(6)-K*eval(hh))*P;
%     P1=inv(P+(eval(hh))'*inv(R)*eval(hh));
%     K=P1*(eval(hh))'*inv(R);
    X1=X'+K*[Z(i,:)'-(eval(h))'];
    Po=P1;
    P1;
    x_filter(:,i+1)=X1;
    cova_filter(:,i+1)=diag(P1);
end
%     X'
figure(1)
plot(x_true(1,:),'r')
hold on
plot(x_filter(1,:),'k');
figure(2)
plot(x_true(2,:),'r')
hold on
plot(x_filter(2,:),'k');
figure(3)
plot(x_true(3,:),'r')
hold on
plot(x_filter(3,:),'k');
figure(4)
plot(x_true(4,:),'r')
hold on
plot(x_filter(4,:),'k');
figure(5)
plot(x_true(5,:),'r')
hold on
plot(x_filter(5,:),'k');
figure(6)
plot(x_true(6,:),'r')
hold on
plot(x_filter(6,:),'k');