

In [344]:

    
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
%pylab inline --no-import-all









    



Populating the interactive namespace from numpy and matplotlib

Kalman filter for altitude estimation from accelerometer and sonar

I) TRAJECTORY

We assume sinusoidal trajectory



In [345]:

    
m = 5000       # timesteps
dt = 1/ 250.0   # update loop at 250Hz

t = np.arange(m) * dt

freq = 0.1      # Hz
amplitude = 0.5 # meter

alt_true = 2 + amplitude * np.cos(2 * np.pi * freq * t)
acc_true = - amplitude * (2 * np.pi * freq)**2 * np.cos(2 * np.pi * freq * t)

plt.plot(t, alt_true)
plt.plot(t, acc_true)
plt.legend(['altitude', 'acceleration'], loc='best')
plt.xlabel('time')









    Out[345]:





<matplotlib.text.Text at 0x7f6d83bb3b00>

II) MEASUREMENTS

Position



In [346]:

    
sonar_sampling_period = 1 / 10.0  # sonar reading at 10Hz

# Sonar noise
sigma_sonar_true = 0.05  # in meters


meas_sonar = alt_true[::(sonar_sampling_period/dt)] + sigma_sonar_true * np.random.randn(m // (sonar_sampling_period/dt))
t_meas_sonar = t[::(sonar_sampling_period/dt)]

plt.plot(t_meas_sonar, meas_sonar, 'or')
plt.plot(t, alt_true)
plt.title("Sonar measurement")
plt.xlabel('time (s)')
plt.ylabel('alt (m)')









    Out[346]:





<matplotlib.text.Text at 0x7f6d83b9aac8>

Acceleration



In [347]:

    
sigma_acc_true = 0.2    # in m.s^-2

acc_bias = 1.5
meas_acc = acc_true + sigma_acc_true * np.random.randn(m) + acc_bias
plt.plot(t, meas_acc, '.')
plt.plot(t, acc_true)
plt.title("Accelerometer measurement")
plt.xlabel('time (s)')
plt.ylabel('acc ($m.s^{-2}$)')









    Out[347]:





<matplotlib.text.Text at 0x7f6d83a77c18>

III) PROBLEM FORMULATION

State vector

$$x_{k} = \left[ \matrix{ z \\ \dot z \\ \zeta } \right] = \matrix{ \text{Altitude} \\ \text{Vertical speed} \\ \text{Accelerometer bias} }$$

Input vector

$$ u_{k} = \left[ \matrix{ \ddot z } \right] = \text{Accelerometer} $$

Formal definition (Law of motion):

$$ x_{k+1} = \textbf{A} \cdot x_{k} + \textbf{B} \cdot u_{k} $$$$ x_{k+1} = \left[ \matrix{ 1 & \Delta t & \frac{1}{2} \Delta t^2 \\ 0 & 1 & \Delta t \\ 0 & 0 & 1 } \right] \cdot \left[ \matrix{ z \\ \dot z \\ \zeta } \right] + \left[ \matrix{ \frac{1}{2} \Delta t^2 \\ \Delta t \\ 0 } \right] \cdot \left[ \matrix{ \ddot z } \right] $$

Measurement

We measure the altitude from sonar

$$ y = H \cdot x $$$$ y = \left[ \matrix{ 1 & 0 & 0 } \right] \cdot x $$

IV) IMPLEMENTATION

Initial state $x_0$



In [348]:

    
x = np.matrix([0.0, 0.0, 0.0]).T
print(x, x.shape)









    



[[ 0.]
 [ 0.]
 [ 0.]] (3, 1)

Initial uncertainty $P_0$



In [349]:

    
P = np.diag([100.0, 100.0, 100.0])
print(P, P.shape)









    



[[ 100.    0.    0.]
 [   0.  100.    0.]
 [   0.    0.  100.]] (3, 3)

Dynamic matrix $A$



In [350]:

    
dt = 1 / 250.0  # Time step between filter steps (update loop at 250Hz)

A = np.matrix([[1.0, dt,  0.5*dt**2],
               [0.0, 1.0, dt ],
               [0.0, 0.0, 1.0]])
print(A, A.shape)









    



[[  1.00000000e+00   4.00000000e-03   8.00000000e-06]
 [  0.00000000e+00   1.00000000e+00   4.00000000e-03]
 [  0.00000000e+00   0.00000000e+00   1.00000000e+00]] (3, 3)

Disturbance Control Matrix $B$



In [351]:

    
B = np.matrix([[0.5*dt**2],
               [dt ],
               [0.0]])
print(B, B.shape)









    



[[  8.00000000e-06]
 [  4.00000000e-03]
 [  0.00000000e+00]] (3, 1)

Measurement Matrix $H$



In [352]:

    
H = np.matrix([[1.0, 0.0, 0.0]])
print(H, H.shape)









    



[[ 1.  0.  0.]] (1, 3)

Measurement noise covariance $R$



In [353]:

    
sigma_sonar = sigma_sonar_true # sonar noise

R = np.matrix([[sigma_sonar**2]])
print(R, R.shape)









    



[[ 0.0025]] (1, 1)

Process noise covariance $Q$



In [354]:

    
from sympy import Symbol, Matrix, latex
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('\Delta t')
s1 = Symbol('\sigma_1')  # drift of accelerometer bias
Qs = Matrix([[0.5*dts**2], [dts], [1.0]])
Qs*Qs.T*s1









    Out[354]:





$$\left[\begin{matrix}0.25 \Delta t^{4} \sigma_{1} & 0.5 \Delta t^{3} \sigma_{1} & 0.5 \Delta t^{2} \sigma_{1}\\0.5 \Delta t^{3} \sigma_{1} & \Delta t^{2} \sigma_{1} & 1.0 \Delta t \sigma_{1}\\0.5 \Delta t^{2} \sigma_{1} & 1.0 \Delta t \sigma_{1} & 1.0 \sigma_{1}\end{matrix}\right]$$



In [355]:

    
sigma_acc_drift = 0.0001

G = np.matrix([[0.5*dt**2],
               [dt],
               [1.0]])
Q = G*G.T*sigma_acc_drift**2

print(Q, Q.shape)









    



[[  6.40000000e-19   3.20000000e-16   8.00000000e-14]
 [  3.20000000e-16   1.60000000e-13   4.00000000e-11]
 [  8.00000000e-14   4.00000000e-11   1.00000000e-08]] (3, 3)

Identity Matrix



In [356]:

    
I = np.eye(3)
print(I, I.shape)









    



[[ 1.  0.  0.]
 [ 0.  1.  0.]
 [ 0.  0.  1.]] (3, 3)

Input



In [357]:

    
u = meas_acc
print(u, u.shape)









    



[ 1.225716    1.0904779   1.23843196 ...,  1.0079401   1.42958025
  0.84930729] (5000,)

V) TEST

Re-init State



In [358]:

    
# State
x[0] = 0.0
x[1] = 0.0
x[2] = 0.0

# Estimate covariance
P[0][0] = 100.0
P[1][0] = 100.0
P[2][0] = 100.0


# Preallocation for Plotting
# estimate
zt = []
dzt= []
zetat=[]

# covariance
Pz = []
Pdz= []
Pzeta=[]

# kalman gain
Kz = []
Kdz= []
Kzeta=[]

Filter loop



In [359]:

    
for filterstep in range(m):    
    
    # ========================
    # Time Update (Prediction)
    # ========================
    
    # Project the state ahead
    x = A*x + B*u[filterstep]
    
    # Project the error covariance ahead
    P = A*P*A.T + Q    
    

    # ===============================
    # Measurement Update (Correction)
    # ===============================
    if filterstep%25 == 0:    
        # Compute the Kalman Gain
        S = H*P*H.T + R
        K = (P*H.T) * np.linalg.pinv(S)
    
        # Update the estimate via z
        Z = meas_sonar[filterstep//25]
        y = Z - (H*x)                            # Innovation or Residual
        x = x + (K*y)
    
        # Update the error covariance
        P = (I - (K*H))*P
    
    # ========================
    # Save states for Plotting
    # ========================
    zt.append(float(x[0]))
    dzt.append(float(x[1]))
    zetat.append(float(x[2]))

    Pz.append(float(P[0,0]))
    Pdz.append(float(P[1,1]))
    Pzeta.append(float(P[2,2]))
    
    Kz.append(float(K[0,0]))
    Kdz.append(float(K[1,0]))
    Kzeta.append(float(K[2,0]))

VI) PLOT

Position $z$



In [360]:

    
plt.figure(figsize=(10,10))
plt.plot(t, zt, color='b')
plt.fill_between(t, np.array(zt) - 10* np.array(Pz), np.array(zt) + 10*np.array(Pz), alpha=0.2, color='b')
plt.plot(t, alt_true, 'g')
plt.plot(t_meas_sonar, meas_sonar, '.r')
#plt.ylim([1.7, 2.3])
plt.ylim([2 - 1.5 * amplitude, 2 + 1.5 * amplitude])
plt.legend(['estimate', 'true altitude', 'sonar reading'])









    Out[360]:





<matplotlib.legend.Legend at 0x7f6d8804a278>

Detail view of position estimate



In [361]:

    
plt.figure(figsize=(10,10))
plt.plot(t, zt, color='b')
plt.fill_between(t, np.array(zt) - 100 * np.array(Pz), np.array(zt) + 100 * np.array(Pz), alpha=0.2, color='b')
plt.plot(t, alt_true, 'g')
plt.plot(t_meas_sonar, meas_sonar, '.r')
plt.xlim([15, 20])
plt.ylim([2 - 1.5 * amplitude, 2 + 1.5 * amplitude])
plt.legend(['estimate', 'true altitude', 'sonar reading'])









    Out[361]:





<matplotlib.legend.Legend at 0x7f6d83953c88>

Accelerometer Bias $\zeta$



In [362]:

    
plt.plot(t, zetat, 'b')
plt.plot([t[0], t[-1]], [-acc_bias, -acc_bias], 'g')
plt.legend(['estimate', 'truth'])









    Out[362]:





<matplotlib.legend.Legend at 0x7f6d839aa128>

Altitude Covariance



In [363]:

    
plt.plot(Pz)









    Out[363]:





[<matplotlib.lines.Line2D at 0x7f6d838a98d0>]



In [363]:



In [363]:



In [363]: