姿态互补滤波

基本思想

加速度计通过重力分量解算倾角，低频准确但含高频噪声；陀螺仪测量角速度，积分后高频准确但低频漂移。互补滤波利用两者频域互补性：加速度计角位移经低通滤波保留低频，陀螺仪角速度先积分再经高通滤波保留高频，求和得到全频段角位移估计。

连续时间一阶互补滤波

设加速度计解算的角位移为 ${\theta }_a(t)$，陀螺仪角速度为 ${\omega }_g(t)$，积分得角位移 ${\theta }_g(t)={\int }_0^t{\omega }_g(\tau )d\tau$。一阶低通滤波器传递函数为 $L(s)=\frac{1}{\tau s+1}$，对应一阶高通为 $H(s)=\frac{\tau s}{\tau s+1}$，满足 $L(s)+H(s)=1$。估计角位移为
$\widehat{\theta }(s)=L(s){\mathrm{\Theta }}_a(s)+H(s){\mathrm{\Theta }}_g(s)=\frac{1}{\tau s+1}{\mathrm{\Theta }}_a(s)+\frac{\tau s}{\tau s+1}{\mathrm{\Theta }}_g(s)$
其中 $\tau$ 为时间常数，单位秒（s），决定互补频率 $1\mathrm{/}\tau$（rad/s）。

负反馈等价形式

将 ${\mathrm{\Theta }}_g(s)=\frac{1}{s}{\mathrm{\Omega }}_g(s)$ 代入并整理，可得等价负反馈结构：
$\widehat{\theta }(s)=\frac{1}{s}[{\mathrm{\Omega }}_g(s)+\frac{1}{\tau }({\mathrm{\Theta }}_a(s)-\widehat{\theta }(s))]$
即陀螺角速度积分，同时用加速度计倾角与估计角之差经增益 $1\mathrm{/}\tau$ 反馈修正漂移。

离散时间差分形式

采用后向差分近似 $s\approx \frac{1-z^{-1}}{T}$，$T$ 为采样周期（s）。一阶低通离散为 $\frac{T\mathrm{/}\tau }{1+T\mathrm{/}\tau -z^{-1}}$，高通为 $\frac{1-z^{-1}}{1+T\mathrm{/}\tau -z^{-1}}$。令 $\alpha =\frac{\tau }{\tau +T}$，则估计值差分方程：
$\widehat{\theta }[k]=\alpha (\widehat{\theta }[k-1]+T{\omega }_g[k])+(1-\alpha ){\theta }_a[k]$
其中 $\widehat{\theta }[k]$ 为当前角度估计，$\widehat{\theta }[k-1]$ 为上一时刻估计，${\omega }_g[k]$ 为当前陀螺角速度，${\theta }_a[k]$ 为当前加速度计角位移，$\alpha \in (0,1)$ 为滤波系数。该式可直接编程实现。

适用范围

适用于加速度计低频可信、陀螺仪高频可信且两者频带互补的姿态估计；要求陀螺零偏和加速度计动态干扰在可接受范围内。时间常数 $\tau$ 需折中：过大则加速度计修正慢、漂移抑制弱；过小则高频噪声抑制不足。

加速度计提供重力主导条件下的倾斜参考，可估计俯仰与横滚，不能单独确定航向；明显平动加速度会干扰该参考。互补滤波须保证坐标、符号、量纲和时间一致，一阶滤波在规定模型和有效频带内近似组合信息，不保证覆盖任意运动的完整频谱。