【互补滤波】加速度计与陀螺仪数据的互补滤波，用于估计角度，公式分析与MATLAB例程。附下载链接

原创已于 2025-07-30 18:30:04 修改 · 1k 阅读

19 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #开发语言 #互补滤波 #角度滤波 #组合导航

于 2025-07-23 09:41:35 首次发布

频域滤波专栏收录该内容

10 篇文章

订阅专栏

在这里插入图片描述

完整的MATLAB互补滤波器（Complementary Filter）实现，用于融合陀螺仪和加速度计数据来估计姿态角。该代码通过仿真演示了互补滤波器的工作原理，并提供了多种参数设置下的性能对比分析。

文章目录

程序介绍
运行结果
MATLAB源代码

程序介绍

概述

这是一个完整的MATLAB互补滤波器（Complementary Filter）实现，用于融合陀螺仪和加速度计数据来估计姿态角。该代码通过仿真演示了互补滤波器的工作原理，并提供了多种参数设置下的性能对比分析。

核心原理

互补滤波器基本思想

互补滤波器是一种简单而有效的传感器融合方法，其核心思想是：

陀螺仪：响应快速，适合捕捉高频运动，但存在累积误差（漂移）
加速度计：长期稳定，适合低频参考，但易受噪声干扰

互补滤波器的数学表达式为：
$θ (k) = α \times [θ (k - 1) + ω g yro \times d t] + (1 - α) \times θ a cc (k)$

其中：

θ(k)：当前时刻的姿态角估计
α：互补滤波器系数（0 < α < 1）
ωgyro：陀螺仪角速度
θacc：加速度计测得的姿态角

代码结构与功能

参数设置

fs = 100;           % 采样频率 100Hz
dt = 1/fs;          % 采样时间间隔
t_total = 10;       % 总仿真时间 10秒
alpha = 0.95;       % 互补滤波器系数

真实姿态角生成
生成一个正弦变化的俯仰角作为真实参考：

pitch_true = 30 * sin(0.5*t) * pi/180;  % 幅值30°，频率0.5Hz

传感器数据仿真

陀螺仪数据：在真实角速度基础上添加偏置和白噪声
加速度计数据：在真实角度基础上添加测量噪声

互补滤波器实现
核心算法循环
逐步融合陀螺仪和加速度计数据
结果分析与可视化

姿态角估计结果对比图
各方法的估计误差对比
RMSE性能统计
频率特性分析
不同α值的性能对比

主要特点

完整的仿真环境：

真实姿态角生成
传感器噪声建模
性能评估指标

多维度分析：

时域分析：姿态角估计结果和误差对比
频域分析：高通/低通滤波器特性
参数分析：不同α值的影响

关键参数说明

α值的影响

α = 0.95（推荐值）：平衡陀螺仪和加速度计的贡献
α → 1：更信任陀螺仪，响应快但易漂移
α → 0：更信任加速度计，稳定但响应慢

噪声参数

gyro_noise_std = 0.1：陀螺仪噪声标准差
gyro_bias = 0.05：陀螺仪偏置
acc_noise_std = 0.05：加速度计噪声标准差

性能评估

代码提供了完整的性能评估体系：

RMSE：均方根误差统计
误差曲线：时域误差变化
频率响应：滤波器特性分析

扩展建议

多轴扩展：扩展到三轴姿态估计（俯仰、横滚、偏航）
自适应α：根据运动状态动态调整α值
卡尔曼滤波对比：与EKF等高级滤波器对比
实时实现：移植到嵌入式系统

运行结果

滤波前后的角度信息曲线、误差曲线：
在这里插入图片描述
频率特性曲线：

命令行窗口输出的结果：

不同 $\alpha$ 时的滤波效果对比：

MATLAB源代码

部分代码如下：

%% 互补滤波器例程 - 姿态估计
% 该例程为使用互补滤波器融合陀螺仪和加速度计数据来估计姿态角，带不同alpha的结果对比
% 作者：matlabfilter
% 2025-07-15/Ver1
clc;clear;close all;
rng(0);
%% 参数设置
fs = 100;           % 采样频率 (Hz)
dt = 1/fs;          % 采样时间间隔 (s)
t_total = 10;       % 总仿真时间 (s)
t = 0:dt:t_total;   % 时间向量
N = length(t);      % 采样点数

% 互补滤波器参数
alpha = 0.95;       % 互补滤波器系数 (0 < alpha < 1)
                    % alpha越大，越信任陀螺仪；alpha越小，越信任加速度计

%% 真实姿态角生成（用于验证）
% 生成一个正弦变化的俯仰角作为真实值
pitch_true = 30 * sin(0.5*t) * pi/180;  % 真实俯仰角 (弧度)

%% 传感器数据仿真
% 陀螺仪数据（角速度）
gyro_noise_std = 0.1;  % 陀螺仪噪声标准差
gyro_bias = 0.05;      % 陀螺仪偏置
gyro_pitch_rate = diff(pitch_true)/dt;  % 真实角速度
gyro_pitch_rate = [gyro_pitch_rate(1) gyro_pitch_rate];  % 补齐长度
gyro_pitch_rate = gyro_pitch_rate + gyro_bias + gyro_noise_std * randn(1, N);  % 添加偏置和噪声

% 加速度计数据（通过重力向量计算得到的角度）
acc_noise_std = 0.05;  % 加速度计噪声标准差
acc_pitch = pitch_true + acc_noise_std * randn(1, N);  % 添加噪声的加速度计角度

%% 互补滤波器实现
pitch_complementary = zeros(1, N);  % 互补滤波器输出
pitch_gyro_only = zeros(1, N);      % 仅陀螺仪积分结果
pitch_acc_only = acc_pitch;         % 仅加速度计结果