modified kalman-filter

簡単のため共分散行列Rを固定していたと思われるが、Rを固定してしまうと、観測値と推定値のずれの情報が、推定誤差共分散行列P_kにいかなくなってしまう。
結果として、得られる推定誤差共分散行列は観測値を反映しないものとなってしまう。
（xyzの分散がすべて同じとなってしまう）

これを変更し、Rを共分散を直接計算するようにした。
この結果、xyzの分散が観測値を反映するようになった。
ただ、これにより発散しやすくなるので、発散してしまう時にはq-varianceの値を大きく変更してみたりすることが必要。