dingdongyan 发表于 2011-8-19 10:48

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

对一个三质量系统进行刚度识别
感觉已经严格按照扩展卡尔曼的定义编程了。还是不行,貌似偶进入了死角,请教各位高手指点
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('滤波后','真值','滤波前')

dingdongyan 发表于 2011-8-19 15:17

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

ChaChing 发表于 2011-8-22 15:08

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

dingdongyan 发表于 2011-8-22 19:03

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

dingdongyan 发表于 2011-8-22 19:04

您太谦虚了!

ChaChing 发表于 2011-8-23 00:12

我用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);
无法试著学习

dingdongyan 发表于 2011-8-23 10:14

哦,我应该贴出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:34

本帖最后由 ChaChing 于 2011-8-24 14:35 编辑

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

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

dingdongyan 发表于 2011-8-24 14:53

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 14:58

本帖最后由 dingdongyan 于 2011-8-24 15:03 编辑

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

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

yangfengdehaizi 发表于 2014-12-11 11:05

楼主的问题解决了吗?
页: [1]
查看完整版本: 扩展卡尔曼参数识别问题请教高手