Kalman FIlter Convergence
Attached is a simple python Kalman filter example of a free-fall object (g=-9.8m/s^2) Alas, I have a problem. The state vector x contains both the position and the velocity but the z vector (measurement) contains only the position.
If I set a wrong initial position value, the algorithm coverages to the true value even with noisy measurements (see picture below)
However, if I sent the wrong initial velocity value, the algorithm does not converge even though the motion model is defined correctly.
Attached is the python code: kalman.py
In your code I see two problems.
You set the Q-Matrix to zero. It means you trust too much in your model and give the filter no chance to improve the estimation through the measurement. Your filter becomes to stiff. You can think of it like a low pass filter with a very big time constant.
In my code I set the Q-Matrix to
Q = np.array([[1,0],[0,0.1]])
The second issue is your measurement noise. You simulate the noisy measurements with R=100 but communicate to the filter R=4. The filter trusts the measurement more than it should be. This issue is not really relevant to your question but still it should be corrected.
Now even if I set the initial velocity to 20, the position estimation works fine.
Here is the estimation for R = 4:
And for R = 100:
UPDATE
The velocity estimation works wrong, because you have some mistakes in your matrix operations. Please note, the matrix multiplication goes through np.dot(), not through *.
Here is a correct result for v0 = 20:
