Created
March 2, 2017 21:18
-
-
Save bigsnarfdude/fe70360c700f8cc9587a589297454fac to your computer and use it in GitHub Desktop.
matplotlib kalman example in python
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
import matplotlib.pyplot as plt | |
plt.rcParams['figure.figsize'] = (10, 8) | |
# intial parameters | |
n_iter = 50 | |
sz = (n_iter,) # size of array | |
x = -0.37727 # truth value (typo in example at top of p. 13 calls this z) | |
z = np.random.normal(x,0.1,size=sz) # observations (normal about x, sigma=0.1) | |
Q = 1e-5 # process variance | |
# allocate space for arrays | |
xhat=np.zeros(sz) # a posteri estimate of x | |
P=np.zeros(sz) # a posteri error estimate | |
xhatminus=np.zeros(sz) # a priori estimate of x | |
Pminus=np.zeros(sz) # a priori error estimate | |
K=np.zeros(sz) # gain or blending factor | |
R = 0.1**2 # estimate of measurement variance, change to see effect | |
# intial guesses | |
xhat[0] = 0.0 | |
P[0] = 1.0 | |
for k in range(1,n_iter): | |
# time update | |
xhatminus[k] = xhat[k-1] | |
Pminus[k] = P[k-1]+Q | |
# measurement update | |
K[k] = Pminus[k]/( Pminus[k]+R ) | |
xhat[k] = xhatminus[k]+K[k]*(z[k]-xhatminus[k]) | |
P[k] = (1-K[k])*Pminus[k] | |
plt.figure() | |
plt.plot(z,'k+',label='noisy measurements') | |
plt.plot(xhat,'b-',label='a posteri estimate') | |
plt.axhline(x,color='g',label='truth value') | |
plt.legend() | |
plt.title('Estimate vs. iteration step', fontweight='bold') | |
plt.xlabel('Iteration') | |
plt.ylabel('Voltage') | |
plt.figure() | |
valid_iter = range(1,n_iter) # Pminus not valid at step 0 | |
plt.plot(valid_iter,Pminus[valid_iter],label='a priori error estimate') | |
plt.title('Estimated $\it{\mathbf{a \ priori}}$ error vs. iteration step', fontweight='bold') | |
plt.xlabel('Iteration') | |
plt.ylabel('$(Voltage)^2$') | |
plt.setp(plt.gca(),'ylim',[0,.01]) | |
plt.show() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment