九轴姿态角度测量传感器数据滤波的MATLAB代码，采用卡尔曼滤波

以下是九轴姿态角度测量传感器数据滤波的MATLAB代码，使用卡尔曼滤波：

clear all;
close all;

%% Load Data
load('sensor_data.mat'); % 九轴姿态角度测量传感器数据

%% Initialize Kalman Filter
dt = 0.01; % 时间步长
A = [1 0 0 dt 0 0 0.5*dt^2 0 0; ...
0 1 0 0 dt 0 0 0.5*dt^2 0; ...
0 0 1 0 0 dt 0 0 0.5*dt^2; ...
0 0 0 1 0 0 dt 0 0; ...
0 0 0 0 1 0 0 dt 0; ...
0 0 0 0 0 1 0 0 dt; ...
0 0 0 0 0 0 1 0 0; ...
0 0 0 0 0 0 0 1 0; ...
0 0 0 0 0 0 0 0 1]; % 状态转移矩阵
B = eye(9); % 控制输入矩阵
C = eye(9); % 观测矩阵
Q = eye(9) * 0.001; % 状态噪声协方差矩阵
R = eye(9) * 0.1; % 观测噪声协方差矩阵
P = eye(9); % 初始状态协方差矩阵
x = zeros(9,1); % 初始状态向量

%% Run Kalman Filter
y = sensor_data';
x_out = zeros(9, length(y));
x_out(:,1) = x;
for i = 2:length(y)
% 预测
x_pred = A * x;
P_pred = A * P * A' + Q;

% 更新
K = P_pred * C' * inv(C * P_pred * C' + R);
x = x_pred + K * (y(:,i) - C * x_pred);
P = (eye(9) - K * C) * P_pred;

x_out(:,i) = x;
end

%% Plot Results
t = (0:length(y)-1) * dt;
figure;
subplot(3,3,1);
plot(t, x_out(1,:), t, sensor_data(:,1)');
legend('Kalman Filter', 'Raw Data');
title('Roll Angle');
subplot(3,3,2);
plot(t, x_out(2,:), t, sensor_data(:,2)');
legend('Kalman Filter', 'Raw Data');
title('Pitch Angle');
subplot(3,3,3);
plot(t, x_out(3,:), t, sensor_data(:,3)');
legend('Kalman Filter', 'Raw Data');
title('Yaw Angle');
subplot(3,3,4);
plot(t, x_out(4,:), t, sensor_data(:,4)');
legend('Kalman Filter', 'Raw Data');
title('Roll Rate');
subplot(3,3,5);
plot(t, x_out(5,:), t, sensor_data(:,5)');
legend('Kalman Filter', 'Raw Data');
title('Pitch Rate');
subplot(3,3,6);
plot(t, x_out(6,:), t, sensor_data(:,6)');
legend('Kalman Filter', 'Raw Data');
title('Yaw Rate');
subplot(3,3,7);
plot(t, x_out(7,:), t, sensor_data(:,7)');
legend('Kalman Filter', 'Raw Data');
title('Roll Acceleration');
subplot(3,3,8);
plot(t, x_out(8,:), t, sensor_data(:,8)');
legend('Kalman Filter', 'Raw Data');
title('Pitch Acceleration');
subplot(3,3,9);
plot(t, x_out(9,:), t, sensor_data(:,9)');
legend('Kalman Filter', 'Raw Data');
title('Yaw Acceleration');