4th order Runge kutta with system of coupled 2nd order ode MATLAB need help i do not know where my algorithm gone wrong

1 次查看（过去 30 天）
Noel Lou
Noel Lou 2016-3-17
编辑： Noel Lou 2016-3-18
MX''=Fn(cosΦ−usinΦ)
MZ''=Fn(sinΦ+ucosΦ)−Mg
IΦ''=Fn(Bxx+uBz)
I tried using Runge-Kutta methods to approximate motion equations in matlab but it turn out wrong. I also tired finding and researching forums and web for solution but to no avail.
Fn,M,θ,u is constant fn/M = 0.866
it seems that i did my coupled runge kutta wrongly. my phi and omega does not change with time. please help
clc; % Clears the screen
clear all;
thete=30;
g= 9.81; %Constant
h=0.01; % step size
tfinal = 3;
N=ceil(tfinal/h); %ceil is round up
t(1) = 0;% initial condition
Vx(1)=0;%initial accleration
X(1)=0;
Vz(1)=0;
Z(1)=20;%initial velocity
fn = 0.866;
Bz = 0.35;
Phi(1)= 30;
W(1)= 0;
%Bxx = ((Z-Z(1))+5.8*cos(thete)+Bz*(sin(Phi)-sin(thete)));
Bxx = 0.5;
%sliding phase
F_X = @(t,X,Vx,Phi) Vx;
F_Z = @(t,Z,Vz,Phi) Vz;
F_Phi= @(t,Phi,W) W;
F_Vx = @(t,X,Vx,Phi)(fn*(cos(Phi)-0.05*(sin(Phi))));
F_Vz = @(t,Z,Vz,Phi)(fn*(sin(Phi)+0.05*(cos(Phi)))-g);
F_W = @(t,Phi,W)((fn*(Bxx+Bz*0.05))/20000);
for i=1:N; % calculation loop
%update time
t(i+1)=t(i)+h;
%update motions main equation
%rotation phase
k_1 = F_X (t(i) ,X(i) ,Vx(i) ,Phi(i));
L_1 = F_Vx(t(i) ,X(i) ,Vx(i) ,Phi(i));
k_2 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(i)+0.5*h*L_1,Phi(i)+0.5*h*L_1);
L_2 = F_Vx(t(i)+0.5*h,X(i)+0.5*h*k_1,Vx(i)+0.5*h*L_1,Phi(i)+0.5*h*L_1);
k_3 = F_X (t(i)+0.5*h,X(i)+0.5*h*k_2,Vx(i)+0.5*h*L_2,Phi(i)+0.5*h*L_2);
L_3 = F_Vx(t(i)+0.5*h,X(i)+0.5*h*k_2,Vx(i)+0.5*h*L_2,Phi(i)+0.5*h*L_2);
k_4 = F_X (t(i)+h ,X(i)+h *k_3,Vx(i)+ h*L_3,Phi(i)+h*L_3);
L_4 = F_Vx(t(i)+h ,X(i)+h *k_3,Vx(i)+ h*L_3,Phi(i)+h*L_3);
k_11 = F_Z (t(i) ,Z(i) ,Vz(i) ,Phi(i));
L_11 = F_Vz(t(i) ,Z(i) ,Vz(i) ,Phi(i));
k_22 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(i)+0.5*h*L_11,Phi(i)+0.5*h*L_11);
L_22 = F_Vz(t(i)+0.5*h,Z(i)+0.5*h*k_11,Vz(i)+0.5*h*L_11,Phi(i)+0.5*h*L_11);
k_33 = F_Z (t(i)+0.5*h,Z(i)+0.5*h*k_22,Vz(i)+0.5*h*L_22,Phi(i)+0.5*h*L_22);
L_33 = F_Vz(t(i)+0.5*h,Z(i)+0.5*h*k_22,Vz(i)+0.5*h*L_22,Phi(i)+0.5*h*L_22);
k_44 = F_Z (t(i)+ h,Z(i)+ h*k_33,Vz(i)+ h*L_33,Phi(i)+h*L_33);
L_44 = F_Vz(t(i)+ h,Z(i)+ h*k_33,Vz(i)+ h*L_33,Phi(i)+h*L_33);
k_5 = F_Phi (t(i) ,Phi(i) ,W(i));
L_5 = F_W (t(i) ,Phi(i) ,W(i));
k_6 = F_Phi (t(i)+0.5*h,Phi(i)+0.5*h*k_5,W(i)+0.5*h*L_5);
L_6 = F_W (t(i)+0.5*h,Phi(i)+0.5*h*k_5,W(i)+0.5*h*L_5);
k_7 = F_Phi (t(i)+0.5*h,Phi(i)+0.5*h*k_6,W(i)+0.5*h*L_6);
L_7 = F_W (t(i)+0.5*h,Phi(i)+0.5*h*k_6,W(i)+0.5*h*L_6);
k_8 = F_Phi (t(i)+ h,Phi(i)+ h*k_7,W(i)+ h*L_7);
L_8 = F_W (t(i)+ h,Phi(i)+ h*k_7,W(i)+ h*L_7);
X(i+1) = X(i) + (h/6)*(k_1+2*k_2+2*k_3+k_4);
Vx(i+1) = Vx(i) + (h/6)*(L_1+2*L_2+2*L_3+L_4);
Z(i+1) = Z(i) + (h/6)*(k_11+2*k_22+2*k_33+k_44);
Vz(i+1) = Vz(i) + (h/6)*(L_11+2*L_22+2*L_33+L_44);
Phi(i+1)= Phi(i) + (h/6)*(k_5+2*k_6+2*k_7+k_8);
W(i+1) = W(i) + (h/6)*(L_5+2*L_6+2*L_7+L_8);
end
figure
plot(X,Z,'--', 'Linewidth', 1.5, 'color', 'red')
xlabel('time')
ylabel('height')
legend('position')
figure
plot(Phi,W,'--', 'Linewidth', 1.5, 'color', 'blue')
xlabel('time')
ylabel('height')
legend('rotation')
figure
plot(t,Vx,'--', 'Linewidth', 1.5, 'color', 'black')
hold on
plot(t,Vz,'--', 'Linewidth', 1.5, 'color', 'yellow')
hold on
plot(t,W,'--','Linewidth',1.5,'color','green')
xlabel('time')
ylabel('height')
legend('Vx','Vz','W')
fprintf('time %.3f\n x %.3f\n z %.3f\n ',t,X,Z);
fprintf('W %.3f\n',W);
fprintf('Phi %.3f\n',Phi);

请先登录，再回答此问题。

回答（1 个）

Ced
Ced 2016-3-18
编辑：Ced 2016-3-18
Why are you dividing F_W by 20000? What kind of system is this? I think your ODEs are working fine, they are just reeeeeally slow.
May I suggest using an integrator function? I think the resulting code is much more readable. Here is your code, rewritten using my own little rk4 integrator so you see what I mean:
function solve_coupled_ode()
clc; % Clears the screen
clear all;
theta =30;
g= 9.81; %Constant
h=0.01; % step size
tfinal = 3;
N=ceil(tfinal/h); %ceil is round up
% parameters
fnM = 0.866; % fn / M
Bz = 0.35;
u = 0.05;
%Bxx = ((Z-Z(1))+5.8*cos(theta)+Bz*(sin(Phi)-sin(thete)));
Bxx = 0.5;
% initial values
x0 = 0;
dx0 = 0;
z0 = 20;
dz0 = 0;
Phi0 = 30;
dPhi0 = 0;
% state vector: y = [ X dX Z dZ Phi dPhi ]'
y0 = [ x0 dx0 z0 dz0 Phi0 dPhi0 ]';
% dynamics
dynamics = @(t,y)f(t,y,fnM,u,Bz,Bxx,g);
% result matrices
y_out = zeros(N,6);
t = zeros(N,1);
% set initial condition and integrate
y_out(1,:) = y0';
for i= 1:N
% update time and dynamics
[ t(i+1), y_out(i+1,:) ] = integrator_rk4(h,dynamics,t(i),y_out(i,:)');
end
% Extract result
X = y_out(:,1);
Vx = y_out(:,2);
Z = y_out(:,3);
Vz = y_out(:,4);
Phi = y_out(:,5);
W = y_out(:,6);
% Plot
figure
plot(X,Z,'--', 'Linewidth', 1.5, 'color', 'red')
xlabel('time')
ylabel('height')
legend('position')
figure
plot(Phi,W,'--', 'Linewidth', 1.5, 'color', 'blue')
xlabel('time')
ylabel('height')
legend('rotation')
figure
plot(t,Vx,'--', 'Linewidth', 1.5, 'color', 'black')
hold on
plot(t,Vz,'--', 'Linewidth', 1.5, 'color', 'yellow')
hold on
plot(t,W,'--','Linewidth',1.5,'color','green')
xlabel('time')
ylabel('height')
legend('Vx','Vz','W')
% fprintf('time %.3f\n x %.3f\n z %.3f\n ',t,X,Z);
% fprintf('W %.3f\n',W);
% fprintf('Phi %.3f\n',Phi);
end
%%%%%%%%%%%%%
%%Dynamics of motion
function dy = f(t,y,fnM,u,Bz,Bxx,g)
% state vector: y = [ X dX Z dZ Phi dPhi ]'
dy = zeros(6,1);
% x, dx
dy(1) = y(2);
dy(2) = fnM*(cos(y(5)) - u*sin(y(5)));
% z, dz
dy(3) = y(4);
dy(4) = fnM*(sin(y(5)) + u*cos(y(5))) - g;
% phi, dphi
dy(5) = y(6);
dy(6) = fnM*(Bxx + u*Bz);
end
%%%%%%%%%%%%%
function [ tout, xout ] = integrator_rk4(dt,dx,t,x)
% function xnext = integrator_rk4(dt,dx,t,x)
%
% This function performs a single Runge-Kutta update step
%
% input:
% dt: timestep
% dx: function handle to dx(t,x)
% t: current time
% x: current state (column vector)
%
% output:
% tout: t + dt
% xout: state at time t + dt
%
tout = t + dt;
dt_half = 0.5*dt;
k1 = dx(t,x);
k2 = dx(t+dt_half,x+dt_half*k1);
k3 = dx(t+dt_half,x+dt_half*k2);
k4 = dx(tout,x+dt*k3);
xout = x + dt*(k1+2*k2+2*k3+k4)/6;
end
Cheers

类别

在 Help Center 和 File Exchange 中查找有关 Programming 的更多信息

标签

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by