compu.m

EKF滤波程序

clear all
xfunc='xfun';
yfunc='yfun';
%=========================================================================
Method = 1;           % Select filter method 
                      % 1=dd1, 2=DD2
%=========================================================================
%nt=3100;
n0=1;
%u1=load('./reacc.dat');
%u2=load('./adjusta.dat');
%u1=load('./beddjusta.dat');
%u2=load('./surdjusta.dat');
u1=load('./10-23-beddjusta(360).dat');
u2=load('./10-23-surdjusta(360).dat');
nt=length(u1);
%u(:,1)=u1(n0:nt)*100.;
u(:,1)=u1(n0:nt)*1;
%y1=load('./14f-a-270.dat');
%y2=load('./base-a-270.dat');
%y1=load('./velocity1.dat');
y1=load('./10-23-velocity1(360).dat');
%y3=load('./displacement1.dat');
y3=load('./10-23-displacement1(360).dat');
%y4=load('./displacement2.dat');

%y(:,1)=y3(1:nt);
%y(:,2)=y4(1:nt);
y(:,1)=y3(n0:nt);              % 位移
y(:,2)=y1(n0:nt);              % 速度 
%y(:,4)=y2(1:nt);
%y(:,1)=y4(1:nt);
%y(:,2)=y2(1:nt);

tidx=[1:size(y,1)]';
u=u/1.;
y=y/1.;
qv=cov(u(:,1));
for i=1:4
    p1(i)=1e+0;
end
%p1(1)=5.0e-4;
%p1(2)=3.0e-2;
p1(1)=cov(y(1:3,1));
p1(2)=cov(y(1:3,2));
p1(3)=p1(1);
p1(4)=p1(2);
for i=5:12
    p1(i)=1e+0;
end
p1(5)=1.0e-5;
%p1(6)=1.0;
%p1(7)=1.0;
p1(8)=1.0e-1;
p1(9)=1.0e-1;
p1(10)=1.0e-3;
%p1(11)=2.0;
p1(12)=1.0e-1;

p0=diag(p1);
%R=diag([1e-2,1e-1]);
%Q=diag([1e-1]);
R=diag([p1(1),p1(2)]);
Q=diag([cov(u(1:3,1))]);
%xx()变量对应的参数为：
%xx(5)-m1,xx(6)-k1,xx(7)-k2,xx(8)-c1,xx(9)-c2,xx(10)-m2,xx(11)-k3,xx(12)-c3;
%xx(1)-u1,xx(2)-u2,xx(3)-v1,xx(4)-v2;

dt=0.005;
x0(1)=y(4,1);
x0(2)=y(4,2);
x0(3)=y(4,1);
x0(4)=y(4,2);
%x0(5)=1.14e-4;
%x0(6)=8.69;
%x0(7)=-1.86;
%x0(8)=6.92e-1;
%x0(9)=-3.06e-1;
%x0(10)=3.04e-2;
%x0(11)=3.17;
%x0(12)=7.19e-1;
x0(5)=2.6e-5;
x0(6)=5.42;
x0(7)=-1.16;
x0(8)=2.53e-1;
x0(9)=-1.12e-1;
x0(10)=6.94e-3;
x0(11)=1.98;
x0(12)=2.63e-1;

xx(:,1)=x0';
%opt.C = [eye(2) zeros(2,10)];
%opt.G = eye(2);
opt.init=[];
[v,d] = eig(p0);      % Cholesky factor of initial state covariance
Sx0 = real(v*sqrt(d));
[v,d] = eig(Q);       % Cholesky factor of process noise covariance
Sv    = real(v*sqrt(d));
[v,d] = eig(R);       % Cholesky factor of measurement noise covariance
Sw    = real(v*sqrt(d));
plot(u)
%pause
%opt.C = [eye(4) zeros(4,36)];
switch Method
case 1,
[xhat,Smat]=dd1(xfunc,yfunc,xx,p0,Q,R,u,y,tidx,opt);
case 2,    
[xhat,Smat]=dd2(xfunc,yfunc,xx,p0,Q,R,u,y,tidx,opt);
otherwise,
    error('no valid method was selected!')
end
    
%输出迭代计算结果
sedi1='./parameters';
fp1=fopen(sedi1,'w+');
for j=1:length(xhat)
    xhat(j,5:12)=xhat(j,5:12)*1.e+8;
    fprintf(fp1,'%12.6e %12.6e %12.6e %12.6e %12.6e %12.6e %12.6e %12.6e\n',xhat(j,5:12));
end

[yhat,rms]=kalmeval('dd1','yfun',R,xhat,Smat,y,tidx,opt);

%画出参数值
noplots=2;
Maxsubplots=4;
samples=nt-n0+1
nx=12;
s=5;
%for p=1:noplots,
  
   %end		 


%输出预测的结果
sedi2='./outresult';
fp2=fopen(sedi2,'w+');
for j=1:nt
    fprintf(fp2,'%10.6f %10.6f\n',yhat(j,:));
end

nh=length(yhat)-1;
for i=n0:nh
    t(i)=i*0.02;
end

%画出振子m1的速度位移图
figure
plot(t(n0:nh),y(n0:nh,1))    %位移图
xlabel('Time (sec)')
ylabel('Displacement (cm)')
set(gca,'xlim',[t(n0),t(nh)]);
grid on
figure
plot(t(n0:nh),y(n0:nh,2))    %速度图
xlabel('Time (sec)')
ylabel('Velocity (cm/s)')
set(gca,'xlim',[t(n0),t(nh)])
grid on

%计算速度和位移的比较精度
ta=0.;
tb=0.;
for i=n0:nh
    xy1(i)=abs(y(i,1)-yhat(i,1));
    xy2(i)=abs(y(i,2)-yhat(i,2));
    ta=ta+xy1(i);
    tb=tb+xy2(i);
end
tdisp=ta/(nh-n0)
tvel=tb/(nh-n0)
    
%画出速度和位移比较图

figure
plot(t(n0:nh),y(n0:nh,1),'r-.',t(n0:nh),yhat(n0:nh,1),'b-')
set(gca,'xlim',[t(n0),t(nh)]);
grid on
xlabel('Time (sec)')
ylabel('Displacement (cm)')
figure
plot(t(n0:nh),y(n0:nh,2),'r-.',t(n0:nh),yhat(n0:nh,2),'b-')
set(gca,'xlim',[t(n0),t(nh)]);
grid on
xlabel('Time (sec)')
ylabel('Velocity (cm/s)')

%画出加速度图
for i=n0:length(u1)
    tt(i)=i*0.02;
end
nt=tt(length(u1));
nt0=tt(n0);
figure
plot(tt,u1)
xlabel('Time (sec)');
ylabel('Acceleration (cm/s/s)');
%set(gca,'xlim',[0 62]);
set(gca,'xlim',[nt0,nt]);
grid on
figure
plot(tt,u2)
xlabel('Time (sec)');
ylabel('Acceleration (cm/s/s)');
%set(gca,'xlim',[0 62]);
set(gca,'xlim',[nt0,nt]);
grid on