|
马上注册,结交更多好友,享用更多功能,让你轻松玩转社区。
您需要 登录 才可以下载或查看,没有账号?我要加入
x
本帖最后由 yoyo0201 于 2011-5-14 22:15 编辑
我要用ode45求一个方程
[t,s]=ode45(@f,0:0.1:50,[-10 10 0 0 0 0 0 0 0 0 0 0]);
plot(t,s(:,1),'b',t,s(:,7),'r');
function xdot=f(t,s)
%定义变量
x=s(1);y=s(2);fai=s(3);u=s(4);v=s(5);r=s(6);xg=s(7);yg=s(8);faig=s(9);ug=s(10);vg=s(11);rg=s(12);
xw=x-xg;yw=y-yg;faiw=fai-faig;uw=u-ug;vw=v-vg;rw=r-rg;
n=[x;y;fai];ng=[xg;yg;faig];nw=n-ng;vw=[uw;vw;rw];
%n=n1+awgn(n1,10,'measured');
J=[cos(fai) -sin(fai) 0;sin(fai) cos(fai) 0;0 0 1];V=[u;v;r];Vg=[ug;vg;rg];
M=[1.0852 0 0;0 2.0575 -0.4087;0 -0.4087 0.2153];
D=[0.0865 0 0;0 0.0762 0.1510;0 0.0151 0.0031];
K=[0.0389 0 0;0 0.0266 0;0 0 0];
P1=[3 0 0;0 3 0;0 0 1];
Q1=[1 0 0;0 1 0;0 0 1];
Q2=[1 0 0;0 1 0;0 0 1];
C1=0.1*eye(3);C2=0.1*eye(3);
d1=10;d2=10;d3=1;d4=1;d5=1;d6=1;
% if t<=20
% faid=10;
% else if 20<t<=30
% faid=5;
% else faid=0;
% end
% end
nd=[0;0;0];nd1=[0;0;0];nd2=[0;0;0];
%求A1,A2,B
A1=-inv(M)*K;
A2=-inv(M)*D;
B=inv(M);
%求P2
P2=lyap(A2,2*Q2');
%求K1,K2
K1=inv(P1)*Q1;
K2=inv(P2)*J'*P1-A1;
%tao中矩阵求解
K11=K1';
k1=K11(:,1);k2=K1(:,2);k3=K1(:,3);
D1=[d1*k1'*k1 0 0;0 d2*k2'*k2 0;0 0 d3*k3'*k3];
z1=ng-nd;
z2=J*Vg+C1*z1+D1*z1-nd1;
Sluog=[0 -rg 0;rg 0 0;0 0 0];
Tvg=[0 0 -vg;0 0 ug;0 0 0];
ff=-(C1+D1)^2*z1+(C1+D1)*z2-nd2+J*A1*ng+J*(A2+Sluog)*Vg;
om1=(C1+D1)*K1+J*K2;om2=J*Tvg;
om11=om1';om22=om2';
w1=om11(:,1);w2=om11(:,2);w3=om11(:,3);w4=om22(:,1);w5=om22(:,2);
D2=[d1*(w1'*w1+w4'*w4) 0 0;0 d5*(w2'*w2+w5'*w5) 0;0 0 d6*(w3'*w3)];
%求控制路tao
tao=-inv(B)*J'*(ff+C2*z2+D2*z2+z1);
n=[x;y;fai];
nd=J*V;
vd=A1*n+A2*V+B*tao;
ngd=J*Vg+K1*nw;
vgd=A1*ng+A2*Vg+B*tao+K2*nw;
xdot=[nd;vd;ngd;vgd];
其中:n=[x;y;fai]是测量值,存在测量误差,因此x,y,fai均存在高斯白噪声,我想请教下在matlab中如何将噪声加进我的x,y,fai中啊?我直接这样加n1=n+randn(3,1);然后运算速度极其慢,貌似还算不出来,请问有什么别的办法可以解决吗?
|
|