This tests a moving robot observing a moving ball.
Previous observation model is uncorrent. Below is the current model. I removed the external input and now the state variable x and observation variable y as well are under global coordinate. The ball decay does not work well, so we might want to replace it with just 1.0.
/* x(t) = A * x(t-1) + v(t-1)
* [ x1(t) ] = [ 1 0 .033 0 ] [ x1(t-1) ] + [ wx1 ]
* [ x2(t) ] [ 0 1 0 .033] [ x2(t-1) ] [ wx2 ]
* [ dx1(t) ] [ 0 0 .99 0 ] [ dx1(t-1) ] [ wdx1 ]
* [ dx2(t) ] [ 0 0 0 .99 ] [ dx2(t-1) ] [ wdx2 ]
*
* y(t) = C * x(t-1) + n(t)
* [ y1(t) ] = [ 1 0 0 0 ] [ x1(t-1) ] + [ vx1 ]
* [ y2(t) ] [ 0 1 0 0 ] [ x2(t-1) ] [ vx2 ]
* [ dx1(t-1) ]
* [ dx2(t-1) ]
*/
