fkanehiro/hrpsys-base — pull request #203 by eisoku9618 (2014-04-23 05:25:11+00:00)
Title: avoid devision by zero

このカルマンフィルタは直立時を想定しているかとは思いますが，転倒などでロボットが仰向け，または，うつ伏せになるときがあり，このとき
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/KalmanFilter/KalmanFilter.cpp#L237
の sx (加速度センサのx軸の値)が +g または -g になり，この行でゼロ除算が発生して制御ループが止まってしまいます．

```TeX
a = atan2( ( sy/g ) / sqrt( 1 - sx/g * sx/g),
( sz/g ) / sqrt( 1 - sx/g * sx/g) );
```

の```sqrt( 1 - sx/g * sx/g)```が0になるのですが，

atan2の両方の引数を同じ値```sqrt( 1 - sx/g * sx/g)```で割っているため，そもそも割る必要がないのではないかと思いました．