# EKF估计相机姿态时零点附近漂移怎么办？

> 原文链接：https://robotics.hellobike.com/pitfalls/2019-04-24-robotics-se-15583
> 频道：pitfalls | 更新日期：2019-04-24 | 标签：auto, stackexchange, kalman-filter, gyroscope, accelerometer, noise
> 核心摘要：相机横滚/俯仰在接近零时漂移，实现问题已解决，并给出按传感器静态采样方差构造Q和R的方法。

# EKF估计相机姿态时零点附近漂移怎么办？

::: v-pre
> 本文由 AI 根据公开内容整理，可能有误，请以原文为准：[KF/ EKF - Modelling and tuning noise matrices and other parameters](https://robotics.stackexchange.com/questions/15583/kf-ekf-modelling-and-tuning-noise-matrices-and-other-parameters)（Robotics Stack Exchange #15583）

**状态**：已解决

## 问题

用C++基于加速度计和陀螺仪通过`KF/EKF`估计相机`roll`、`pitch`、`yaw`时，大多数姿态结果良好，但`roll`或`pitch`接近零时曲线出现漂移或过零模式。

## 环境

状态估计 / EKF 滤波器

## 解决办法

1. 过零漂移的关键问题与实现有关，原提问者表示已修复，但未说明具体代码改动。
2. 构造`Q`：在已知位置和姿态、无运动条件下采集大量陀螺仪数据，分别计算x/y/z轴均值和标准差`noise_gx`、`noise_gy`、`noise_gz`。
3. 将`noise_gx`、`noise_gy`、`noise_gz`代入陀螺仪角速度到欧拉角速度的转换公式，并乘以时间量；将得到的三个值平方，作为`Q`的三个对角元素，其余元素置零，前提是假设状态变量之间无协方差。
4. `P`的初始不确定性根据系统信息和试错设置。
5. 构造`R`：先把加速度计读数转换到角度等与观测变量相同的空间，再计算标准差`noise_ax`、`noise_ay`、`noise_az`，平方后填入`R`的对角线。

原问题作者：skr；采纳答案作者：skr。原内容依 CC BY-SA 4.0 授权，本页摘要同样依 CC BY-SA 4.0 授权。
:::

## 原始参考来源 (Sources)

- [Robotics Stack Exchange #15583：KF/ EKF - Modelling and tuning noise matrices and other parameters](https://robotics.stackexchange.com/questions/15583/kf-ekf-modelling-and-tuning-noise-matrices-and-other-parameters)