编一段生成一个惯导数据集合的MATLAB代码

以下是一段生成惯导数据集合的MATLAB代码：

% 生成惯导数据集合
N = 1000; % 数据点数
dt = 0.01; % 时间步长
t = (0:N-1)*dt; % 时间序列
gi = 5; % 陀螺仪噪声标准差，单位为度/秒
ai = 0.2; % 加速度计噪声标准差，单位为米/秒^2
mi = 0.1; % 磁强计噪声标准差，单位为高斯

% 生成随机陀螺仪数据
w = gi*randn(N,3); % 陀螺仪数据，单位为度/秒

% 生成随机加速度计数据
a = ai*randn(N,3); % 加速度计数据，单位为米/秒^2

% 生成随机磁强计数据
m0 = [1;0;0]; % 地球磁场方向，单位矢量
m = repmat(m0',N,1); % 磁强计数据，单位为高斯
theta = 2*pi*rand(N,1); % 随机旋转角度
R = axang2rotm([cos(theta/2) sin(theta/2).*randn(N,2)]); % 随机旋转矩阵
m = (R*m')'; % 旋转磁强计数据
m = m + mi*randn(N,3); % 加入噪声

% 初始化姿态、位置和速度
q0 = [1;0;0;0]; % 初始姿态四元数
v0 = [0;0;0]; % 初始位置速度
r0 = [0;0;0];
r = zeros(N,3); % 位置，单位为米
v = zeros(N,3); % 速度，单位为米/秒
q = zeros(N,4); % 姿态四元数
q(1,:) = q0';
r(1,:) = r0';
v(1,:) = v0';

% 进行积分
for i = 2:N
    % 计算姿态更新量
    omega = w(i,:)'*pi/180; % 陀螺仪数据转换为弧度/秒
    qn = q(i-1,:)';
    dq = [cos(norm(omega*dt)/2); sin(norm(omega*dt)/2)/norm(omega*dt)*omega*dt]*qn;
    q(i,:) = dq';
    
    % 计算加速度和磁矢场
    g = [0;0;-9.81]; % 重力加速度，单位为米/秒^2
    a(i,:) = a(i,:) + g'; % 加速度数据加入重力加速度
    m(i,:) = m(i,:)/norm(m(i,:)); % 磁强计数据单位化
    m_b = quatrotate(qn',m0')'; % 姿态应用于地球磁场，计算出测量磁矢场
    m_n = quatrotate(qn',m(i,:)')'; % 姿态应用于磁强计测量数据，得到真实磁矢场
    
    % 计算速度和位置更新量
    f = quatrotate(qn',a(i,:)')'; % 姿态应用于加速度计测量数据，得到真实加速度
    v(i,:) = v(i-1,:) + f*dt; % 计算速度更新量
    r(i,:) = r(i-1,:) + v(i-1,:)*dt + 0.5*f*dt^2; % 计算位置更新量
end

% 绘制结果
figure;
subplot(2,2,1);
plot(t,w);
title('陀螺仪');
xlabel('时间（秒）');
ylabel('角速度（度/秒）');
subplot(2,2,2);
plot(t,a);
title('加速度计');
xlabel('时间（秒）');
ylabel('加速度（米/秒^2）');
subplot(2,2,3);
plot(t,m);
title('磁强计');
xlabel('时间（秒）');
ylabel('磁场（高斯）');
subplot(2,2,4);
plot3(r(:,1),r(:,2),r(:,3));
title('位置');
xlabel('x（米）');
ylabel('y（米）');
zlabel('z（米）');