import numpy as np import pandas as pd import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from IPython.display import YouTubeVideo from scipy.stats import norm %matplotlib inline fw=10 # figure width YouTubeVideo("tIIJME8-au8") P = 100.0*np.eye(9) fig = plt.figure(figsize=(6, 6)) im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary')) plt.title('Initial Covariance Matrix $P$') ylocs, ylabels = plt.yticks() # set the locations of the yticks plt.yticks(np.arange(10)) # set the locations and labels of the yticks plt.yticks(np.arange(9),('$x$', '$y$', '$z$', '$\dot x$', '$\dot y$', '$\dot z$', '$\ddot x$', '$\ddot y$', '$\ddot z$'), fontsize=22) xlocs, xlabels = plt.xticks() # set the locations of the yticks plt.xticks(np.arange(7)) # set the locations and labels of the yticks plt.xticks(np.arange(9),('$x$', '$y$', '$z$', '$\dot x$', '$\dot y$', '$\dot z$', '$\ddot x$', '$\ddot y$', '$\ddot z$'), fontsize=22) plt.xlim([-0.5,8.5]) plt.ylim([8.5, -0.5]) from mpl_toolkits.axes_grid1 import make_axes_locatable divider = make_axes_locatable(plt.gca()) cax = divider.append_axes("right", "5%", pad="3%") plt.colorbar(im, cax=cax) plt.tight_layout() dt = 1.0/100.0 # Time Step between Filter Steps, 100Hz A = np.matrix([[1.0, 0.0, 0.0, dt, 0.0, 0.0, 1/2.0*dt**2, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 1/2.0*dt**2, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0, 1/2.0*dt**2], [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, dt], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) print(A.shape) H = np.matrix([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]) print(H, H.shape) rp = 5.0**2 # Noise of Position Measurement R = np.matrix([[rp, 0.0, 0.0], [0.0, rp, 0.0], [0.0, 0.0, rp]]) print(R, R.shape) fig = plt.figure(figsize=(4, 4)) im = plt.imshow(R, interpolation="none", cmap=plt.get_cmap('binary')) plt.title('Measurement Noise Covariance Matrix $R$') ylocs, ylabels = plt.yticks() # set the locations of the yticks plt.yticks(np.arange(4)) # set the locations and labels of the yticks plt.yticks(np.arange(3),('$x$', '$y$', '$z$'), fontsize=22) xlocs, xlabels = plt.xticks() # set the locations of the yticks plt.xticks(np.arange(4)) # set the locations and labels of the yticks plt.xticks(np.arange(3),('$x$', '$y$', '$z$'), fontsize=22) plt.xlim([-0.5,2.5]) plt.ylim([2.5, -0.5]) from mpl_toolkits.axes_grid1 import make_axes_locatable divider = make_axes_locatable(plt.gca()) cax = divider.append_axes("right", "5%", pad="3%") plt.colorbar(im, cax=cax) plt.tight_layout() from sympy import Symbol, Matrix from sympy.interactive import printing printing.init_printing() dts = Symbol('\Delta t') Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[0.5*dts**2],[dts],[dts],[dts],[1.0],[1.0],[1.0]]) Qs*Qs.T sa = 0.5 G = np.matrix([[1/2.0*dt**2], [1/2.0*dt**2], [1/2.0*dt**2], [dt], [dt], [dt], [1.0], [1.0], [22.0]]) # because we want to estimate g, # here we use a huge value to give the # Kalman Filter the possibility to # 'breath' Q = G*G.T*sa**2 print(Q.shape) fig = plt.figure(figsize=(6, 6)) im = plt.imshow(Q, interpolation="none", cmap=plt.get_cmap('binary')) plt.title('Process Noise Covariance Matrix $Q$') ylocs, ylabels = plt.yticks() # set the locations of the yticks plt.yticks(np.arange(10)) # set the locations and labels of the yticks plt.yticks(np.arange(9),('$x$', '$y$', '$z$', '$\dot x$', '$\dot y$', '$\dot z$', '$\ddot x$', '$\ddot y$', '$\ddot z$'), fontsize=22) xlocs, xlabels = plt.xticks() # set the locations of the yticks plt.xticks(np.arange(7)) # set the locations and labels of the yticks plt.xticks(np.arange(9),('$x$', '$y$', '$z$', '$\dot x$', '$\dot y$', '$\dot z$', '$\ddot x$', '$\ddot y$', '$\ddot z$'), fontsize=22) plt.xlim([-0.5,8.5]) plt.ylim([8.5, -0.5]) from mpl_toolkits.axes_grid1 import make_axes_locatable divider = make_axes_locatable(plt.gca()) cax = divider.append_axes("right", "5%", pad="3%") plt.colorbar(im, cax=cax) plt.tight_layout() I = np.eye(9) print(I, I.shape) data = pd.read_csv('Ball.csv') data.head() [Xm, Ym, Zm, Xr, Yr, Zr] = data.T.values fig = plt.figure(figsize=(12,6)) ax = fig.add_subplot(111, projection='3d') ax.scatter(Xm, Ym, Zm, c='gray') ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') plt.title('Ball Trajectory observed from Computer Vision System (with Noise)') #ax.w_xaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) # Axis equal max_range = np.array([Xm.max()-Xm.min(), Ym.max()-Ym.min(), Zm.max()-Zm.min()]).max() / 3.0 mean_x = Xm.mean() mean_y = Ym.mean() mean_z = Zm.mean() ax.set_xlim(mean_x - max_range, mean_x + max_range) ax.set_ylim(mean_y - max_range, mean_y + max_range) ax.set_zlim(mean_z - max_range, mean_z + max_range) #plt.savefig('BallTrajectory-Computervision.png', dpi=150, bbox_inches='tight') measurements = np.vstack((Xm,Ym,Zm)) m = len(measurements[0]) # short it print(measurements.shape) x = np.matrix([0.0, 0.0, 1.0, 10.0, 0.0, 0.0, 0.0, 0.0, -15.0]).T print(x, x.shape) # Preallocation for Plotting xt = [] yt = [] zt = [] dxt= [] dyt= [] dzt= [] ddxt=[] ddyt=[] ddzt=[] Zx = [] Zy = [] Zz = [] Px = [] Py = [] Pz = [] Pdx= [] Pdy= [] Pdz= [] Pddx=[] Pddy=[] Pddz=[] Kx = [] Ky = [] Kz = [] Kdx= [] Kdy= [] Kdz= [] Kddx=[] Kddy=[] Kddz=[] hitplate=False for filterstep in range(m): # Model the direction switch, when hitting the plate if x[2]<0.01 and not hitplate: x[5]=-x[5] hitplate=True # Time Update (Prediction) # ======================== # Project the state ahead x = A*x #+ B*u # we have no Control Input # Project the error covariance ahead P = A*P*A.T + Q # Measurement Update (Correction) # =============================== # Compute the Kalman Gain S = H*P*H.T + R K = (P*H.T) * np.linalg.pinv(S) # Update the estimate via z Z = measurements[:,filterstep].reshape(H.shape[0],1) y = Z - (H*x) # Innovation or Residual x = x + (K*y) # Update the error covariance P = (I - (K*H))*P # Save states for Plotting xt.append(float(x[0])) yt.append(float(x[1])) zt.append(float(x[2])) dxt.append(float(x[3])) dyt.append(float(x[4])) dzt.append(float(x[5])) ddxt.append(float(x[6])) ddyt.append(float(x[7])) ddzt.append(float(x[8])) Zx.append(float(Z[0])) Zy.append(float(Z[1])) Zz.append(float(Z[2])) Px.append(float(P[0,0])) Py.append(float(P[1,1])) Pz.append(float(P[2,2])) Pdx.append(float(P[3,3])) Pdy.append(float(P[4,4])) Pdz.append(float(P[5,5])) Pddx.append(float(P[6,6])) Pddy.append(float(P[7,7])) Pddz.append(float(P[8,8])) Kx.append(float(K[0,0])) Ky.append(float(K[1,0])) Kz.append(float(K[2,0])) Kdx.append(float(K[3,0])) Kdy.append(float(K[4,0])) Kdz.append(float(K[5,0])) Kddx.append(float(K[6,0])) Kddy.append(float(K[7,0])) Kddz.append(float(K[8,0])) fig = plt.figure(figsize=(fw,9)) plt.subplot(211) plt.title('Estimated State (elements from vector $x$)') plt.plot(range(len(measurements[0])),dxt, label='$\dot x$') plt.plot(range(len(measurements[0])),dyt, label='$\dot y$') plt.plot(range(len(measurements[0])),dzt, label='$\dot z$') plt.legend(loc='best',prop={'size':22}) plt.subplot(212) plt.plot(range(len(measurements[0])),ddxt, label='$\ddot x$') plt.plot(range(len(measurements[0])),ddyt, label='$\ddot y$') plt.plot(range(len(measurements[0])),ddzt, label='$\ddot z$') plt.xlabel('Filter Step (100 = 1 Second)') plt.ylabel('$m/s^2$') plt.legend(loc='best',prop={'size':22}) print('Estimated g= %.2f m/s2' % ddzt[-1]) fig = plt.figure(figsize=(fw,9)) plt.subplot(311) plt.plot(range(len(measurements[0])),Px, label='$x$') plt.plot(range(len(measurements[0])),Py, label='$y$') plt.plot(range(len(measurements[0])),Pz, label='$z$') plt.title('Uncertainty (Elements from Matrix $P$)') plt.legend(loc='best',prop={'size':22}) plt.subplot(312) plt.plot(range(len(measurements[0])),Pdx, label='$\dot x$') plt.plot(range(len(measurements[0])),Pdy, label='$\dot y$') plt.plot(range(len(measurements[0])),Pdz, label='$\dot z$') plt.legend(loc='best',prop={'size':22}) plt.subplot(313) plt.plot(range(len(measurements[0])),Pddx, label='$\ddot x$') plt.plot(range(len(measurements[0])),Pddy, label='$\ddot y$') plt.plot(range(len(measurements[0])),Pddz, label='$\ddot z$') plt.xlabel('Filter Step') plt.ylabel('') plt.legend(loc='best',prop={'size':22}) fig = plt.figure(figsize=(fw,9)) plt.plot(range(len(measurements[0])),Kx, label='Kalman Gain for $x$') plt.plot(range(len(measurements[0])),Ky, label='Kalman Gain for $y$') plt.plot(range(len(measurements[0])),Kz, label='Kalman Gain for $z$') plt.plot(range(len(measurements[0])),Kdx, label='Kalman Gain for $\dot x$') plt.plot(range(len(measurements[0])),Kdy, label='Kalman Gain for $\dot y$') plt.plot(range(len(measurements[0])),Kdz, label='Kalman Gain for $\dot z$') plt.plot(range(len(measurements[0])),Kddx, label='Kalman Gain for $\ddot x$') plt.plot(range(len(measurements[0])),Kddy, label='Kalman Gain for $\ddot y$') plt.plot(range(len(measurements[0])),Kddz, label='Kalman Gain for $\ddot z$') plt.xlabel('Filter Step') plt.ylabel('') plt.title('Kalman Gain (the lower, the more the measurement fullfill the prediction)') plt.legend(loc='best',prop={'size':18}); fig = plt.figure(figsize=(6, 6)) im = plt.imshow(P, interpolation="none", cmap=plt.get_cmap('binary')) plt.title('Covariance Matrix $P$ (after %i Filtersteps)' % m) ylocs, ylabels = plt.yticks() # set the locations of the yticks plt.yticks(np.arange(10)) # set the locations and labels of the yticks plt.yticks(np.arange(9),('$x$', '$y$', '$z$', '$\dot x$', '$\dot y$', '$\dot z$', '$\ddot x$', '$\ddot y$', '$\ddot z$'), fontsize=22) xlocs, xlabels = plt.xticks() # set the locations of the yticks plt.xticks(np.arange(7)) # set the locations and labels of the yticks plt.xticks(np.arange(9),('$x$', '$y$', '$z$', '$\dot x$', '$\dot y$', '$\dot z$', '$\ddot x$', '$\ddot y$', '$\ddot z$'), fontsize=22) plt.xlim([-0.5,8.5]) plt.ylim([8.5, -0.5]) from mpl_toolkits.axes_grid1 import make_axes_locatable divider = make_axes_locatable(plt.gca()) cax = divider.append_axes("right", "5%", pad="3%") plt.colorbar(im, cax=cax) plt.tight_layout() fig = plt.figure(figsize=(fw,4)) plt.plot(xt,zt, label='Kalman Filter Estimate') plt.scatter(Xm,Zm, label='Measurements', c='gray', s=30) plt.plot(Xr, Zr, label='Real') plt.title('Estimate of Ball Trajectory (Elements from State Vector $x$)') plt.legend(loc='best',prop={'size':14}) plt.axhline(0, color='k') plt.axis('equal') plt.xlabel('X ($m$)') plt.ylabel('Y ($m$)') plt.ylim([0, 1]); plt.savefig('Kalman-Filter-CA-Ball-StateEstimated.png', dpi=72, bbox_inches='tight') fig = plt.figure(figsize=(16,9)) ax = fig.add_subplot(111, projection='3d') ax.plot(xt,yt,zt, label='Kalman Filter Estimate') ax.plot(Xr, Yr, Zr, label='Real') ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z') ax.legend() plt.title('Ball Trajectory estimated with Kalman Filter') # Axis equal max_range = np.array([Xm.max()-Xm.min(), Ym.max()-Ym.min(), Zm.max()-Zm.min()]).max() / 3.0 mean_x = Xm.mean() mean_y = Ym.mean() mean_z = Zm.mean() ax.set_xlim(mean_x - max_range, mean_x + max_range) ax.set_ylim(mean_y - max_range, mean_y + max_range) ax.set_zlim(mean_z - max_range, mean_z + max_range) plt.savefig('Kalman-Filter-CA-Ball-Trajectory.png', dpi=150, bbox_inches='tight') dist = np.sqrt((Xm-xt)**2 + (Ym-yt)**2 + (Zm-zt)**2) print('Estimated Position is %.2fm away from ball position.' % dist[-1])