Revive old KalmanFilter code

https://github.com/fkanehiro/hrpsys-base/pull/296
で古いKalmanFilterのコードがきえていますが、古いバージョンのコードとして残しておきたいのと、
移行がおわりきれてないため選べるようにしました。

@eisoku9618 さん確認をお願いします。