Adding LANDMARK:  0
[[      1.          0.          0.          1.          0.   ]
 [      0.          1.          0.          0.          1.   ]
 [      0.          0.          1.       -363.144     235.829]
 [      1.          0.       -363.144  133026.924  -86385.233]
 [      0.          1.        235.829  -86385.233   56104.556]]
Adding LANDMARK:  1
[[      1.          0.          0.          1.          0.          1.          0.   ]
 [      0.          1.          0.          0.          1.          0.          1.   ]
 [      0.          0.          1.       -363.144     235.829    -221.981    -369.439]
 [      1.          0.       -363.144  133026.924  -86385.233   80612.296  134159.726]
 [      0.          1.        235.829  -86385.233   56104.556  -52349.588  -87123.345]
 [      1.          0.       -221.981   80612.296  -52349.588   49709.925   82722.371]
 [      0.          1.       -369.439  134159.726  -87123.345   82722.371  137678.455]]
INV: [[-1.003]
 [ 0.011]]
Kalman Gain * Inv
[[ 0.   ]
 [ 0.   ]
 [ 0.002]
 [-0.915]
 [-0.004]
 [-0.395]
 [-0.657]]
Updating with LANDMARK:  0
[[    1.        0.        0.        1.        0.        1.        0.   ]
 [    0.        1.        0.        0.        1.        0.        1.   ]
 [    0.        0.        0.004    -3.661     2.368    -0.913    -1.519]
 [    1.        0.       -3.661  3264.553 -2114.675   813.724  1352.6  ]
 [    0.        1.        2.368 -2114.675  1374.308  -525.762  -874.015]
 [    1.        0.       -0.913   813.724  -525.762   636.811  1050.994]
 [    0.        1.       -1.519  1352.6    -874.015  1050.994  1754.458]]

---------------------FORWARD MOVEMENT-------------------
INV: [[-39.371]
 [ -0.078]]
Kalman Gain * Inv
[[-0.273]
 [ 4.462]
 [-0.007]
 [-0.382]
 [ 0.341]
 [ 0.268]
 [ 0.318]]
Updating with LANDMARK:  1
[[  1.084   0.908  -0.001   1.      0.001   1.055   0.033]
 [  0.909  54.894  -0.085   0.      0.983  -0.889   0.475]
 [ -0.001  -0.086   0.     -0.     -0.      0.002   0.001]
 [  0.92    1.297  -0.002   1.007   0.049   0.873  -0.114]
 [  0.071  -0.164   0.002   0.      1.002   0.113   1.066]
 [  1.056  -0.909   0.001   1.     -0.      1.088   0.054]
 [  0.066  -0.075   0.002  -0.007   0.951   0.106   1.102]]
INV: [[ 4.677]
 [-0.086]]
Kalman Gain * Inv
[[ 0.076]
 [-0.006]
 [ 0.   ]
 [-0.004]
 [ 0.004]
 [ 0.074]
 [ 0.046]]
Updating with LANDMARK:  0
[[ 1.005  0.023 -0.     1.007  0.045  0.871 -0.113]
 [ 0.     1.    -0.     0.001  1.006 -0.027  0.979]
 [ 0.     0.     0.     0.     0.    -0.    -0.   ]
 [ 1.004  0.021 -0.     1.007  0.046  0.883 -0.106]
 [-0.003  0.978 -0.     0.     1.005  0.102  1.058]
 [ 1.008  0.049  0.     1.006  0.042  0.881 -0.104]
 [ 0.003  1.023  0.    -0.003  0.978 -0.022  1.004]]