instest.m

对输入的信号进行卡尔曼滤波。内插信号

oumiga=6/24;
oud=oumiga*sin(.785);
oun=oumiga*cos(.785);
g=9.8;


a0 = [0  oud    0    1 0 0 0 0,
   -oud  0    oun   0 0 1 0 0,
     0 -oun    0    0 1 0 0 0,
     0 0 0          0 0 0 0 0,
     0 0 0          0 0 0 0 0,
     0 0 0          0 0 0 0 0,
     0 0 0          0 0 0 0 0,
     0 0 0          0 0 0 0 0];
 
b0=[ 1 0 0,
    0 1 0,
    0 0 1,
    0 0 0,
    0 0 0,
    0 0 0,
    0 0 0,
    0 0 0];

c0= [0 g 0 0 0 0 1 0,
    -g 0 0 0 0 0 0 1];

d0=0;

q0=[0.6^2  0   0,
    0  0.6^2  0,
    0   0  0.6^2];

r0=[(.0001*g)^2 0,
     0   (.0001*g)^2];
 
P=[(60)^2   0   0   0    0    0     0   0,
    0  (60)^2   0   0    0    0     0   0,
    0   0  (60)^2   0    0    0     0   0,
    0   0   0    0.6^2   0    0     0   0,
    0   0   0     0   0.6^2   0     0   0,
    0   0   0     0    0   0.6^2    0   0,
    0   0   0   0    0  0  (0.0001*g)^2   0,
    0   0   0   0    0  0   0  (0.0001*g)^2]; 
x0=[60 60 60  0.6 0.6 0.6 0.0001*g 0.0001*g];

dimf=size(b0); %output dimention x*y of b0
noinp=dimf(2);
dimc=size(c0);
noout=dimc(1);
w1=sqrt(q0);

t=[0:0.1:99*0.1]';
w=randn(length(t),noinp)*sqrt(q0);

nu=rand(length(t),noout)*sqrt(r0);

[y,x]=lsim(a0,b0,c0,d0,w,t,x0);
y=x*c0'+nu;

[L P E]=lqe(a0,b0,c0,q0,r0);

[a3 ,b3,c3,d3]=estim(a0,b0,c0,d0,L);

[y1,xh]=lsim(a3,b3,c3,d3,y,t);

subplot(221);
plot(t,x(:,1),t,xh(:,1));
subplot(222);
plot(t,x(:,2),t,xh(:,2));
subplot(223);
plot(t,x(:,3),t,xh(:,3));

xerr=x-xh;
subplot(224);
plot(t,xerr);