换一个方法，采用四阶龙格库塔法求解

好的，以下是采用四阶龙格库塔法求解的六自由度运动模型的Matlab代码：

% 定义初始状态
x0 = [0; 0; 0; 0; 0; 0];  % 初始位置和姿态
v0 = [0; 0; 0];  % 初始速度
w0 = [0; 0; 0];  % 初始角速度
state0 = [x0; v0; w0];

% 定义模型参数
m = 1000;  % 质量
I = diag([1000, 2000, 3000]);  % 转动惯量
g = 9.81;  % 重力加速度
d = 10;  % 阻力系数
l = 5;  % 质心到旋转中心的距离

% 定义控制量
T = 5000;  % 推力
tau = [0; 0; 0];  % 扭矩

% 定义时间间隔和总时间
dt = 0.01;  % 时间间隔
t = 0:dt:10;  % 总时间

% 定义欧拉角度运动方程
function dx = euler_ang_motion(t, x, m, I, g, d, l, T, tau)
    % 获取状态量
    pos = x(1:3);
    vel = x(4:6);
    euler = x(7:9);
    omega = x(10:12);

    % 计算旋转矩阵
    R = eul2rotm(euler,'XYZ');

    % 计算加速度和角加速度
    F = [0; 0; T] - R*[0; 0; m*g] - d*vel;
    a = F / m;
    alpha = inv(I)*(tau - cross(omega,I*omega));

    % 计算下一时刻的状态
    k1 = [vel; a; omega; alpha];
    k2 = [vel+0.5*dt*a; a; omega+0.5*dt*alpha; alpha];
    k3 = [vel+0.5*dt*a; a; omega+0.5*dt*alpha; alpha];
    k4 = [vel+dt*a; a; omega+dt*alpha; alpha];
    next_state = x + dt/6*(k1+2*k2+2*k3+k4);

    % 返回状态向量
    dx = next_state;
end

% 模拟运动
state = zeros(length(t),12);
state(1,:) = state0';
for i = 2:length(t)
    state(i,:) = euler_ang_motion(t(i), state(i-1,:)', m, I, g, d, l, T, tau)';
end

% 绘制运动轨迹和姿态
figure
plot3(state(:,1),state(:,2),state(:,3))
xlabel('x')
ylabel('y')
zlabel('z')
title('Underwater Vehicle Motion')

figure
plot(t, state(:,7:9))
xlabel('Time (s)')
ylabel('Euler Angle (rad)')
legend('Roll','Pitch','Yaw')
title('Underwater Vehicle Attitude')

在这个代码中，我们采用四阶龙格库塔法求解运动模型。函数`eul2rotm`用于计算欧拉角对应的旋转矩阵。在`euler_ang_motion`函数中，我们先计算出加速度和角加速度，然后使用四阶龙格库塔法计算下一时刻的状态。使用一个循环来模拟整个运动过程，并在运动结束后使用Matlab的绘图函数来绘制水下航行器的运动轨迹和姿态。