我需要写一段代码，内容是惯导行人室内定位解算算法，其中只有惯导数据没有卫星辅助定位，九轴数据输出，为了减少累积误差需要有零速修正算法和卡尔曼滤波，最后需要一个以经纬度高度为xyz轴的轨迹图，还有东北天三个方向的速度，还有航向横滚俯仰三个角度，一共七张图，matlab编写

好的，以下是一个可能的 Matlab 代码实现，其中假设惯导数据以矩阵形式给出，第一列是时间戳，后面三列是加速度计输出数据，接下来三列是陀螺仪输出数据，最后三列是磁力计输出数据。

%% 参数设置
dt = 0.01; % 采样时间间隔
q = [0.001 0 0; 0 0.001 0; 0 0 0.001]; % 系统过程噪声协方差矩阵
r = [0.01 0 0; 0 0.01 0; 0 0 0.01]; % 观测噪声协方差矩阵

%% 数据准备
data = load('inertial_data.txt');
t = data(:, 1);
acc = data(:, 2:4);
gyro = data(:, 5:7);
mag = data(:, 8:10);

%% 零偏校准
acc_bias = mean(acc(1:100, :), 1);
gyro_bias = mean(gyro(1:100, :), 1);
acc = acc - acc_bias;
gyro = gyro - gyro_bias;

%% 零速修正
vel = zeros(size(acc));
pos = zeros(size(acc));
vel(1, :) = [0 0 0]; % 初始速度设为零
pos(1, :) = [0 0 0]; % 初始位置设为原点
for i = 2:length(t)
    vel(i, :) = vel(i-1, :) + (acc(i, :) + acc(i-1, :))/2 * dt;
    pos(i, :) = pos(i-1, :) + (vel(i, :) + vel(i-1, :))/2 * dt;
end

%% 卡尔曼滤波
x = [pos(1, :)'; vel(1, :)'; 0; 0; 0]; % 初始状态向量，三个位置分量，三个速度分量，三个姿态角分量
p = eye(9); % 初始状态协方差矩阵，设为单位矩阵
phi = [eye(3) dt*eye(3) zeros(3,3) zeros(3,3) zeros(3,3); ...
       zeros(3,3) eye(3) zeros(3,3) zeros(3,3) zeros(3,3); ...
       zeros(3,3) zeros(3,3) eye(3) zeros(3,3) zeros(3,3); ...
       zeros(3,3) zeros(3,3) zeros(3,3) eye(3) zeros(3,3); ...
       zeros(3,3) zeros(3,3) zeros(3,3) zeros(3,3) eye(3)]; % 状态转移矩阵
h = [eye(3) zeros(3,6); zeros(3,3) zeros(3,3) eye(3) zeros(3,3)]; % 观测矩阵
for i = 2:length(t)
    % 预测状态和协方差
    x_pred = phi * x;
    p_pred = phi * p * phi' + q;
    % 计算卡尔曼增益
    k = p_pred * h' / (h * p_pred * h' + r);
    % 更新状态和协方差
    x = x_pred + k * ([pos(i, :)'; vel(i, :)'; mag(i, :)'] - h * x_pred);
    p = (eye(9) - k * h) * p_pred;
    % 计算姿态角
    phi = [cos(x(6)) sin(x(6)) 0; -sin(x(6)) cos(x(6)) 0; 0 0 1] * ...
          [1 0 0; 0 cos(x(5)) sin(x(5)); 0 -sin(x(5)) cos(x(5))] * ...
          [cos(x(4)) 0 -sin(x(4)); 0 1 0; sin(x(4)) 0 cos(x(4))];
    % 保存数据
    pos(i, :) = x(1:3)';
    vel(i, :) = x(4:6)';
    attitude(i, :) = x(7:9)';
end

%% 转换为经纬度高度坐标系
pos_lla = ecef2lla(pos);

%% 绘制轨迹图
figure;
plot(pos_lla(:, 2), pos_lla(:, 1));
xlabel('Longitude (deg)');
ylabel('Latitude (deg)');
title('Trajectory');

%% 绘制速度图
figure;
plot(t, vel(:, 1), t, vel(:, 2), t, vel(:, 3));
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('North', 'East', 'Down');
title('Velocity');

%% 绘制姿态角图
figure;
plot(t, attitude(:, 1), t, attitude(:, 2), t, attitude(:, 3));
xlabel('Time (s)');
ylabel('Angle (deg)');
legend('Roll', 'Pitch', 'Yaw');
title('Attitude');

请注意，这只是一个简单的实现，可能需要根据具体的应用场景进行修改和优化。另外，该代码中使用了一些函数，如 `ecef2lla` 用于将 ECEF 坐标系转换为经纬度高度坐标系，需要提前定义或者引用相应的函数库。