记一次陀螺仪采样值滤波算法的测试过程

今天帮同事设计了一个算法,针对如下问题:

有一个类似陀螺仪的硬件,定期上报角度数据(0~360°变化,可能跨越0点),但是有一个难以消除的硬件故障,会固定跳变±err(比如约为5°),而两次采样值之间正常的最大角度为θ(比如约为0.5°),且θ远小于δ。现在需要设计一个滤波算法消去跳变的采样点。

算法的基本思路很简单,假设硬件开机时的基准角度是正确的,而后的采样周期开始计算一阶差分Δ,如果Δ>θ,则修正一个-err,如果Δ<-θ,则修正一个+err。不过过零点的情况需要仔细考虑,但是也不是太复杂。

我花了一个小时给同事写了一个详细的算法说明,然后他讨论过后他把算法翻译成代码,结果测试是就是不对,我也看了他写的代码,虽然不是直接用我写的公式,但看起来应该是等价的,没啥问题。快要内分泌失调的时候,另外一个同事发现代码中少写了一个else,导致某两个平行的分支变成串行了。

这个事情的启示是:

  1. 低级错误和高级的算法错误一样,造成的后果都是糟糕的;
  2. 有两个if并行时,最好不要偷懒不写else,不写应该100%确认理由;
  3. 太疲惫时还是不要调试的好,或者找个脑子清醒的人帮忙看看,毕竟,在过去的30几个小时,我才睡了不到6个小时。