matlab绘制imu运动轨迹的程序，imu数据存放在txt文件中

以下是一个绘制IMU运动轨迹的Matlab程序：

% 读取IMU数据
data = load('imu_data.txt');
% 从数据中提取出加速度和角速度数据
acc = data(:, 1:3);
gyro = data(:, 4:6);
% 采样频率为100Hz，计算时间间隔dt
dt = 1/100;
% 初始化位置、速度和姿态
pos = zeros(size(data, 1), 3);
vel = zeros(size(data, 1), 3);
quat = zeros(size(data, 1), 4);
% 初始化加速度计和磁力计偏差
acc_bias = zeros(1, 3);
mag_bias = zeros(1, 3);
% 设置重力向量
g = [0 0 -9.81]';
% 初始化上一时刻的姿态和重力向量
last_quat = [1 0 0 0]';
last_gravity = g;

for i = 2:size(data, 1)
    % 计算角速度
    gyro_i = deg2rad(gyro(i, :));
    % 计算加速度
    acc_i = acc(i, :)';
    % 去除加速度计偏差
    acc_i = acc_i - acc_bias';
    % 计算磁力计
    mag_i = [0 0 0]';
    % 去除磁力计偏差
    mag_i = mag_i - mag_bias';
    % 使用Mahony滤波器计算姿态
    [quat_i, last_gravity] = mahony_filter(quat(i-1, :)', gyro_i, acc_i, mag_i, dt, last_gravity);
    quat(i, :) = quat_i';
    % 使用姿态计算加速度在世界坐标系下的方向
    acc_world = quatrotate(quatinv(quat_i'), acc_i')';
    % 计算速度和位置
    vel(i, :) = vel(i-1, :) + (acc_world - g')*dt;
    pos(i, :) = pos(i-1, :) + vel(i, :)*dt;
end

% 绘制运动轨迹
figure;
plot3(pos(:, 1), pos(:, 2), pos(:, 3), 'LineWidth', 2);
xlabel('X');
ylabel('Y');
zlabel('Z');
grid on;

在本程序中，我们使用了Mahony滤波器来计算IMU的姿态。这个滤波器是一种基于四元数的滤波器，可以有效地去除加速度计和磁力计的噪声，并且不需要进行磁校准。如果你希望了解Mahony滤波器的详细原理，可以参考以下论文：

Mahony, R., Hamel, T., & Pflimlin, J. M. (2008). Nonlinear complementary filters on the special orthogonal group. IEEE Transactions on automatic control, 53(5), 1203-1218.