具备自适应能力的互补滤波算法，通过实时评估传感器噪声水平，动态调整陀螺仪与加速度计之间的融合比例，在外部扰动或噪声变化的环境下保持稳定的姿态估计精度。
相比传统的固定参数互补滤波方法，无需人工调参，能够根据数据特性自动优化滤波权重，从而显著提升姿态估计的鲁棒性、精度与适应性。
文章目录
程序简介
程序给出了基于噪声协方差自适应的互补滤波算法（Adaptive Complementary Filter, ACF）和标准互补滤波算法在姿态角估计中的性能对比效果
程序首先构建一个正弦变化的真实角度信号作为参考真值，同时生成包含漂移与随机噪声 的陀螺仪数据，以及带有外部加速度干扰与噪声的加速度计数据。仿真时在 3--6 秒内人为加入外部加速度扰动，用于测试滤波器在复杂环境下的稳健性。
- 标准互补滤波器采用固定权重系数
- 自适应互补滤波根据传感器噪声方差动态调整融合权重的策略，在每个采样周期内，分别计算陀螺仪与加速度计在滑动窗口内的方差估计，当加速度计噪声较大时，滤波器自动提高陀螺仪信号权重；反之则更多依赖加速度计结果。该方法在动态干扰下可实时平衡两类传感器信号的可信度。
运行结果
滤波前后的角度估计曲线对比：
误差对比曲线：
RMSE对比：
MATLAB源代码
部分代码如下：
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.csdn.net/callmeup/category_13061396.html
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者