纯惯导解算matlab

以下是一个简单的纯惯导解算Matlab程序示例代码，实现了欧拉角法（姿态解算）和纯积分法（位置解算）：

% 纯惯导解算Matlab程序示例代码
% 实现欧拉角法（姿态解算）和纯积分法（位置解算）
% 引用[1]: https://blog.csdn.net/qq_38906523/article/details/103654684

clc; clear all; close all;

% 采样频率
fs = 100;

% 时间序列
t = 0:1/fs:10;

% 初始位置
x0 = [0; 0; 0];

% 初始速度
v0 = [0; 0; 0];

% 初始姿态
phi0 = [0; 0; 0];

% 初始角速度
w0 = [0; 0; 0];

% 加速度
a = [0; 0; 9.8];

% 角速度
w = [0.1; 0.2; 0.3];

% 欧拉角法（姿态解算）
phi = zeros(3,length(t));
phi(:,1) = phi0;
for i = 2:length(t)
    phi(:,i) = phi(:,i-1) + w * (1/fs);
end

% 纯积分法（位置解算）
x = zeros(3,length(t));
x(:,1) = x0;
v = zeros(3,length(t));
v(:,1) = v0;
for i = 2:length(t)
    v(:,i) = v(:,i-1) + a * (1/fs);
    x(:,i) = x(:,i-1) + v(:,i-1) * (1/fs);
end

% 绘图
figure(1);
subplot(3,1,1);
plot(t,phi(1,:),'r');
title('Roll');
xlabel('Time(s)');
ylabel('Angle(rad)');
subplot(3,1,2);
plot(t,phi(2,:),'g');
title('Pitch');
xlabel('Time(s)');
ylabel('Angle(rad)');
subplot(3,1,3);
plot(t,phi(3,:),'b');
title('Yaw');
xlabel('Time(s)');
ylabel('Angle(rad)');

figure(2);
subplot(3,1,1);
plot(t,x(1,:),'r');
title('X');
xlabel('Time(s)');
ylabel('Position(m)');
subplot(3,1,2);
plot(t,x(2,:),'g');
title('Y');
xlabel('Time(s)');
ylabel('Position(m)');
subplot(3,1,3);
plot(t,x(3,:),'b');
title('Z');
xlabel('Time(s)');
ylabel('Position(m)');