扩展卡尔曼参数识别问题请教高手
对一个三质量系统进行刚度识别感觉已经严格按照扩展卡尔曼的定义编程了。还是不行，貌似偶进入了死角，请教各位高手指点
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();
K=;
C=;
B=;
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,,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=';
M=diag();
K=;
C=;
A=;
B=];
%%%%%%求增益所需矩阵AF，Hex%%%%%%
Af=; zeros(3,9)];
AF=eye(9)+Af*dt;
Hex=;
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.10.13 0.1210.5-2100 100 200];
x0=x0';
pex=eye(9);
pex(7:9,7:9)=eye(3)*1000;
for j=1:length(t)
%%%预测%%%
a=;
y=x0+int(a,j*dt,(j+1)*dt);%向前推算状态变量,状态预测
y=subs(y,,x0(7:9));
Af=subs(Af,,);
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('滤波后','真值','滤波前') 哪位高手帮我看看呀？我自己是在找不出原因。。。。 个人水平/专业有限, 总感觉LZ的程序好乱, 一直报错! 啊，我用的是2008matlab的版本，没有出现运行错误呢。谢谢您的关注 您太谦虚了！ 我用R2009a版本试!
??? Undefined function or method 'ode4' for input arguments of type 'function_handle'.
Error in ==> zzz at 13
x = ode4(@myfunc,,x0);
若改为ode45又出现
??? Index exceeds matrix dimensions.
Error in ==> zzz at 14
x0=x(:,2);
无法试著学习 哦，我应该贴出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 = 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,);
% 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 于 2011-8-24 14:35 编辑
找空档试执行下, 没报错只是loop(1:length(t))花了不少时间!
个人专业/时间有限, 无法细看, 只是好奇一定得用符号吗!?
for j=1:length(t)
%%%预测%%%
Af=subs(Af,,);
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 于 2011-8-24 15:03 编辑
不用符号的也试过了，运行时间比较短，结果差不多。那个用符号的，有篇论文里提到可以那么积分，所以尝试了一番。
file:///G:/1.figure
补充内容 (2013-2-21 17:23):
为什么现在不能发帖，不能回复，不能留言，不能签到了我？ 楼主的问题解决了吗？
页:
[1]