扩展卡尔曼参数识别问题请教高手

dingdongyan

对一个三质量系统进行刚度识别
感觉已经严格按照扩展卡尔曼的定义编程了。还是不行，貌似偶进入了死角，请教各位高手指点
function dy=myfunc(t,x)
m1=20;      m2=20;          m3=20;
c1=2;       c2=2;           c3=2;
k1=1000000;     k2=10000000;         k3=10000000;
% syms k1,k2,k3;
M=diag([m1 m2 m3]);
K=[k1 -k1 0;-k1 k1+k2 -k2;0 -k2 k2+k3];
C=[c1 -c1 0;-c1 c1+c2 -c2;0 -c2 c2+c3];
B=[0;0;0];
dy = zeros(6,1);
dy(1:3)=x(4:6);
dy(4:6)=-M\K*x(1:3)-M\C*x(4:6)+inv(M)*B;


%%%%%%%%%%%%%%%%%%%信号生成%%%%%%%%%%%%%%%%%
fs=2048;
dt=1/fs;
N=round(1*fs);
t=(0:N+1)/fs;
t=t';
N=20;
z=zeros(N,6);
x0=[ 0.1005    0.1004    0.0946    0.9998    0.5005  -22.4725 ];
x0=x0';
z=zeros(1,6);
N=length(t);
for k=1:N-1
    x = ode4(@myfunc,[k*dt (k+1)*dt],x0);
    x0=x(:,2);
    z(k,:)=x0';
end
z(N,:)=x0';
zm=awgn(z,50,'measured');%%加噪声
measure=zm(:,1:3);%%位移作为观测向量
%%%%%%%%%%%%%%%%%%%%%kalman滤波%%%%%%%%%%%%%%%%%%%
m1=20;      m2=20;          m3=20;
c1=50;       c2=50;           c3=60;
% k1=1000000;     k2=10000000;         k3=10000000;

syms k1 k2 k3;%%待识别参数
syms x1 x2 x3;
x=[x1 x2 x3]';

M=diag([m1 m2 m3]);
K=[k1 -k1 0;-k1 k1+k2 -k2;0 -k2 k2+k3];
C=[c1 -c1 0;-c1 c1+c2 -c2;0 -c2 c2+c3];

A=[zeros(3) eye(3); -M\K -M\C];
B=[zeros(3,1);M\[0;0;1]];

%%%%%%求增益所需矩阵AF，Hex%%%%%%
Af=[zeros(3) eye(3) zeros(3);-M\K -M\C [-M\diff(K,k1)*x -M\diff(K,k2)*x -M\diff(K,k3)*x]; zeros(3,9)];
AF=eye(9)+Af*dt;
Hex=[eye(3) zeros(3)  zeros(3)];
R=cov(measure);
Q=zeros(9,9);
Q(1:6,1:6)=eye(6)*1e-3;
Q(7:9,7:9)=eye(3)*1e-2;

x0=[ 0.1  0.13   0.12  1  0.5  -2  100 100 200];
x0=x0';
pex=eye(9);
pex(7:9,7:9)=eye(3)*1000;

for j=1:length(t)
    %%%预测%%%
    a=[x0(4:6);-M\K*x0(1:3)-M\C*x0(4:6);zeros(3,1)];
    y=x0+int(a,j*dt,(j+1)*dt);%向前推算状态变量,状态预测
    y=subs(y,[k1,k2,k3],x0(7:9));
   
    Af=subs(Af,[x1 x2 x3 k1 k2 k3],[x0(1:3) x0(7:9)]);
    AF=eye(9)+Af*dt;
    pex1=AF*pex*AF'+Q;%向前推算误差协方差
  
    %%增益%%%
    kg=pex1*Hex'*inv(Hex*pex1*Hex'+R);
   
    %%%更新估计%%%%%
    yy=y+kg*(measure(j,:)'-Hex*y);
    ye(j,:)=yy';
    x0=yy;
    %%%误差协方差更新%%%
    pex=pex1-kg*Hex*pex1;
   
end


plot(t,ye(:,1))
hold on
plot(t,z(:,1),'r:')
plot(t,zm(:,1),'g:')
legend('滤波后','真值','滤波前')

dingdongyan

哪位高手帮我看看呀？我自己是在找不出原因。。。。

ChaChing

个人水平/专业有限, 总感觉LZ的程序好乱, 一直报错!

dingdongyan

啊，我用的是2008matlab的版本，没有出现运行错误呢。谢谢您的关注

dingdongyan

您太谦虚了！

ChaChing

我用R2009a版本试!

??? Undefined function or method 'ode4' for input arguments of type 'function_handle'.
Error in ==> zzz at 13
    x = ode4(@myfunc,[k*dt (k+1)*dt],x0);

若改为ode45又出现

??? Index exceeds matrix dimensions.
Error in ==> zzz at 14
    x0=x(:,2);

无法试著学习

dingdongyan

哦，我应该贴出ode4的函数，谢谢ChaChing这么关心，期待您的指点
function Y = ode4(odefun,tspan,y0,varargin)
%ODE4 Solve differential equations with a non-adaptive method of order 4.
% Y = ODE4(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates
% the system of differential equations y' = f(t,y) by stepping from T0 to
% T1 to TN. Function ODEFUN(T,Y) must return f(t,y) in a column vector.
% The vector Y0 is the initial conditions at T0. Each row in the solution
% array Y corresponds to a time specified in TSPAN.
%
% Y = ODE4(ODEFUN,TSPAN,Y0,P1,P2...) passes the additional parameters
% P1,P2... to the derivative function as ODEFUN(T,Y,P1,P2...).
%
% This is a non-adaptive solver. The step sequence is determined by TSPAN
% but the derivative function ODEFUN is evaluated multiple times per step.
% The solver implements the classical Runge-Kutta method of order 4.
%
% Example
% tspan = 0:0.1:20;
% y = ode4(@vdp1,tspan,[2 0]);
% plot(tspan,y(:,1));
% solves the system y' = vdp1(t,y) with a constant step size of 0.1,
% and plots the first component of the solution.
%

if ~isnumeric(tspan)
error('TSPAN should be a vector of integration steps.');
end

if ~isnumeric(y0)
error('Y0 should be a vector of initial conditions.');
end

h = diff(tspan);
if any(sign(h(1))*h <= 0)
error('Entries of TSPAN are not in order.')
end

try
f0 = feval(odefun,tspan(1),y0,varargin{:});
catch
msg = ['Unable to evaluate the ODEFUN at t0,y0. ',lasterr];
error(msg);
end

y0 = y0(:); % Make a column vector.
if ~isequal(size(y0),size(f0))
error('Inconsistent sizes of Y0 and f(t0,y0).');
end

neq = length(y0);
N = length(tspan);
Y = zeros(neq,N);
F = zeros(neq,4);

Y(:,1) = y0;
for i = 2:N
ti = tspan(i-1);
hi = h(i-1);
yi = Y(:,i-1);
F(:,1) = feval(odefun,ti,yi,varargin{:});
F(:,2) = feval(odefun,ti+0.5*hi,yi+0.5*hi*F(:,1),varargin{:});
F(:,3) = feval(odefun,ti+0.5*hi,yi+0.5*hi*F(:,2),varargin{:});
F(:,4) = feval(odefun,tspan(i),yi+hi*F(:,3),varargin{:});
Y(:,i) = yi + (hi/6)*(F(:,1) + 2*F(:,2) + 2*F(:,3) + F(:,4));
end

ChaChing

找空档试执行下, 没报错只是loop(1:length(t))花了不少时间!

个人专业/时间有限, 无法细看, 只是好奇一定得用符号吗!?

dingdongyan

for j=1:length(t)
    %%%预测%%%
   
    Af=subs(Af,[x1 x2 x3 k1 k2 k3],[x0(1:3) x0(7:9)]);
    AF=eye(9)+Af*dt;
   
%     y=AF*x0;%向前推算状态变量,状态预测
    A=subs(A,[ k1 k2 k3],x0(7:9));

    y=x0+A*x0*dt;
   
   
    pex1=AF*pex*AF'+Q;%向前推算误差协方差
  
    %%增益%%%
    kg=pex1*Hex'*inv(Hex*pex1*Hex'+R);
   
    %%%更新估计%%%%%
    yy=y+kg*(measured(j,:)'-Hex*y);
    ye(j,:)=yy';
    x0=yy;
    %%%误差协方差更新%%%
    pex=pex1-kg*Hex*pex1;
    end

dingdongyan

不用符号的也试过了，运行时间比较短，结果差不多。那个用符号的，有篇论文里提到可以那么积分，所以尝试了一番。
file:///G:/1.figure

补充内容 (2013-2-21 17:23):
为什么现在不能发帖，不能回复，不能留言，不能签到了我？

yangfengdehaizi

楼主的问题解决了吗？