【MATLAB例程】基于噪声协方差自适应的互补滤波器方法vs标准互补滤波，用于融合加速度计和陀螺仪数据，估计角度

具备自适应能力的互补滤波算法，通过实时评估传感器噪声水平，动态调整陀螺仪与加速度计之间的融合比例，在外部扰动或噪声变化的环境下保持稳定的姿态估计精度。
相比传统的固定参数互补滤波方法，无需人工调参，能够根据数据特性自动优化滤波权重，从而显著提升姿态估计的鲁棒性、精度与适应性。

程序简介

程序给出了基于噪声协方差自适应的互补滤波算法（Adaptive Complementary Filter, ACF）和标准互补滤波算法在姿态角估计中的性能对比效果

程序首先构建一个正弦变化的真实角度信号作为参考真值，同时生成包含漂移与随机噪声的陀螺仪数据，以及带有外部加速度干扰与噪声的加速度计数据。仿真时在 3–6 秒内人为加入外部加速度扰动，用于测试滤波器在复杂环境下的稳健性。

• 标准互补滤波器采用固定权重系数
• 自适应互补滤波根据传感器噪声方差动态调整融合权重的策略，在每个采样周期内，分别计算陀螺仪与加速度计在滑动窗口内的方差估计，当加速度计噪声较大时，滤波器自动提高陀螺仪信号权重；反之则更多依赖加速度计结果。该方法在动态干扰下可实时平衡两类传感器信号的可信度。

运行结果

滤波前后的角度估计曲线对比：

误差对比曲线：

RMSE对比：

MATLAB源代码

部分代码如下：

%% 基于噪声协方差自适应的互补滤波器方法与标准互补滤波对比
% 作者：matlabfilter
% 2025-10-26/Ver1

clear; close all; clc;
rng(0);
%% 生成仿真数据
dt = 0.01; % 采样时间
t = 0:dt:10; % 时间向量
N = length(t);

% 真实角度 (正弦运动)
true_angle = 30*sin(2*pi*0.2*t);
true_angular_vel = 30*2*pi*0.2*cos(2*pi*0.2*t);

% 陀螺仪数据 (带漂移和噪声)
gyro_noise = 0.5; % 陀螺仪噪声标准差
gyro_drift = 0.1*t; % 陀螺仪漂移
gyro_data = true_angular_vel + gyro_drift + gyro_noise*randn(1,N);

% 加速度计数据 (带噪声和外部加速度干扰)

完整代码：
https://blog.youkuaiyun.com/callmeup/category_13061396.html

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者