利用加速度计和陀螺仪采集的数据分别求姿态角，在利用卡尔曼滤波器融合得到最终的姿态角 matlab代码实例

以下是一个简单的 MATLAB 代码实例，用于利用加速度计和陀螺仪采集的数据分别求姿态角，并使用卡尔曼滤波器融合得到最终的姿态角。请注意，此代码仅供参考，具体实现可能因设备、传感器和应用程序而异。

% Initialize variables and constants
dt = 0.01; % Sampling time
angleAcc = [0, 0]; % Angle calculated from accelerometer
angleGyro = [0, 0]; % Angle calculated from gyroscope
angleKalman = [0, 0]; % Angle calculated using Kalman filter
P = eye(2); % Covariance matrix
Q = [0.01, 0; 0, 0.01]; % Process noise covariance matrix
R = [0.1, 0; 0, 0.1]; % Measurement noise covariance matrix
H = eye(2); % Observation matrix

% Loop through data
for i = 1:length(data)
    % Calculate angle from accelerometer
    angleAcc(1) = atan2(data(i, 2), data(i, 3)) * 180 / pi;
    angleAcc(2) = atan2(-data(i, 1), sqrt(data(i, 2)^2 + data(i, 3)^2)) * 180 / pi;
    
    % Calculate angle from gyroscope
    angleGyro(1) = angleGyro(1) + data(i, 4) * dt;
    angleGyro(2) = angleGyro(2) + data(i, 5) * dt;
    
    % Calculate Kalman gain
    K = P * H' * inv(H * P * H' + R);
    
    % Combine accelerometer and gyroscope measurements using Kalman filter
    angleKalman = angleGyro + K * (angleAcc - angleGyro);
    P = (eye(2) - K * H) * P * (eye(2) - K * H)' + K * R * K';
    
    % Display results
    disp(['Acc: ', num2str(angleAcc(1)), ', ', num2str(angleAcc(2))]);
    disp(['Gyro: ', num2str(angleGyro(1)), ', ', num2str(angleGyro(2))]);
    disp(['Kalman: ', num2str(angleKalman(1)), ', ', num2str(angleKalman(2))]);
end

在此代码中，我们假设数据是一个 $n \times 5$ 的矩阵，其中每行包含 $x$、$y$、$z$ 轴的加速度计数据和 $x$、$y$ 轴的陀螺仪数据。我们还假设加速度计和陀螺仪的输出已经进行了校准和单位转换。在实际应用中，可能需要进行更多的前期处理和校准，以确保准确性和稳定性。

在主循环中，我们首先使用加速度计数据计算姿态角。具体来说，我们使用 $atan2$ 函数计算 $x$ 和 $y$ 轴的旋转角度，并将结果转换为度数。然后，我们使用陀螺仪数据计算姿态角，通过将上一次的角度加上角速度乘以采样时间。请注意，陀螺仪数据可能会有漂移，这可能会导致姿态角的偏移。我们将在下一步中使用卡尔曼滤波器来校正这些偏移。

接下来，我们使用卡尔曼滤波器将加速度计和陀螺仪数据结合起来，以获得更准确和稳定的姿态角。我们首先计算卡尔曼增益 $K$，它基于当前估计的误差协方差和测量噪声协方差。然后，我们将加速度计和陀螺仪数据结合起来，使用卡尔曼增益校正姿态角。最后，我们更新误差协方差矩阵 $P$，以便下一次迭代使用。

最后，我们显示加速度计、陀螺仪和卡尔曼滤波器计算出的姿态角。在实际应用中，可能需要将这些数据发送到其他设备或处理器，以进行进一步的处理或控制。