diff options
| -rw-r--r-- | filter-kalman/kalman_simulation.py | 4 |
1 files changed, 2 insertions, 2 deletions
diff --git a/filter-kalman/kalman_simulation.py b/filter-kalman/kalman_simulation.py index 011349ec..3e792212 100644 --- a/filter-kalman/kalman_simulation.py +++ b/filter-kalman/kalman_simulation.py @@ -61,7 +61,7 @@ for k in range(1,N): # measurement update K[k] = Pminus[k] * H.T * np.linalg.inv( H * Pminus[k] * H.T + R ) xhat[k] = xhatminus[k] + K[k] * (measurement[k] - H * xhatminus[k]) - P[k] = ( I - K[k] ) * Pminus[k] + P[k] = ( I - K[k]*H ) * Pminus[k] t = np.arange(N) * dt plt.figure() @@ -73,4 +73,4 @@ plt.plot(t, x, 'r-', label='ground truth') plt.subplot(2,1,2) plt.plot(t, xhat[:,1,0],'g-',label='velocity estimate') -plt.show()
\ No newline at end of file +plt.show() |