2013年12月29日 星期日

Kalman Filter Example: Water Tank

以下的 water tank example is from Cornell


Case 1: Constant water level with some measurement noise.
If we use the following Kalman filter with q=0.0001 and r=0.1, we get the following result that match with the true water level.


Case 2: 如果實際的 water level 是隨時間增加,但仍然用以上的 Kalman filter。意即 model error, 會發生什麼狀況?
可以看到 Kalman filter output 的誤差愈來愈大而發散,原因是 Kalman filter model 和實際有落差。如何解決這個問題: (1) 放大 q, state prediction 有更多的自由度; (2) 用 2D model 包含速度。

先看 (1), q=0.01 and r=0.1
有改善但仍然不夠。Try q=r=0.1, 可以追上但 (a) noise 增加; (b) q 需要 try and error, 不實用。
q=1 and r=0.1, 基本上 follow measurement (Kg~1).


正解是用 2D model 包含速度:


qf = 0.00001 and r=0.1 注意 Kalman filter 會自動收斂到對的速度, regardless initial condition.

qf = 0.00001 and r=0.1 for constant tank level. (velocity=0)
Case 3: 如果 water level 是 oscillation, 2D with position and velocity 會如何?
1. Estimated state is smoother but lags behind true value. 這和前面用 static model constant velocity 產生的 lag 類似。代表 model 有問題。
2. The amplitude of the estimated state is getting smaller and smaller.
This is because the model is slowly converging to what it thinks is the truth... a constant level, which is accurate over time.

需要 extended Kalman filter 才能解以上的問題。

追蹤者