

In [729]:

    
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 [730]:

    
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[730]:





<matplotlib.text.Text at 0x7f24b0942c88>

II) MEASUREMENTS

Position



In [731]:

    
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[731]:





<matplotlib.text.Text at 0x7f24b088e9b0>

Acceleration



In [732]:

    
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[732]:





<matplotlib.text.Text at 0x7f24b0883a90>

III) PROBLEM FORMULATION

State vector

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

Input vector

$$ u_{k} = 0 $$

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 \\ 0 & 1 & \Delta t & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1} \right] \cdot \left[ \matrix{ z \\ \dot z \\ \ddot z \\ \zeta } \right] + 0 $$

Measurement

We measure the altitude from sonar

$$ y_{sonar} = H_{sonar} \cdot x $$$$ y_{sonar} = \left[ \matrix{ 1 & 0 & 0 & 0 } \right] \cdot x $$

We measure the biased vertical acceleration from accelerometer

$$ y_{acc} = H_{acc} \cdot x $$$$ y_{acc} = \left[ \matrix{ 0 & 0 & 1 & 1 } \right] \cdot x $$

IV) IMPLEMENTATION

Initial state $x_0$



In [733]:

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









    



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

Initial uncertainty $P_0$



In [734]:

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









    



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

Dynamic matrix $A$



In [735]:

    
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],
               [0.0, 1.0, dt,        0.0 ],
               [0.0, 0.0, 1.0,       0.0],
               [0.0, 0.0, 0.0,       1.0]])
print(A, A.shape)









    



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

Measurement Matrix $H$



In [736]:

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









    



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



In [737]:

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









    



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

Measurement noise covariance $R$



In [738]:

    
sigma_sonar = sigma_sonar_true # sonar noise

R_sonar = np.matrix([[sigma_sonar**2]])

print(R_sonar, R_sonar.shape)









    



[[ 0.0025]] (1, 1)



In [739]:

    
sigma_acc = sigma_acc_true # accelerometer noise

R_acc = np.matrix([[sigma_acc**2]])

print(R_acc, R_acc.shape)









    



[[ 0.04]] (1, 1)

Process noise covariance $Q$



In [740]:

    
from sympy import Symbol, Matrix, latex
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('\Delta t')
s1 = Symbol('\sigma_1')  # inexact assumption of constant acceleration
s2 = Symbol('\sigma_2')  # drift of accelerometer bias
Qv = Matrix([[0.5*dts**2*s1], [dts*s1], [s1], [0.0]])
Qs = Qv*Qv.T
Qs[3,3] = s2**2
Qs









    



/usr/lib/python3.4/site-packages/IPython/core/formatters.py:239: FormatterWarning: Exception in image/png formatter: [Errno 12] Cannot allocate memory
  FormatterWarning,






    Out[740]:





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



In [741]:

    
sigma_acc_process = 0.05
sigma_acc_drift = 0.0001

G = np.matrix([[0.5*dt**2 * sigma_acc_process],
               [dt * sigma_acc_process],
               [1.0 * sigma_acc_process],
               [0.0]])
Q = G*G.T
Q[3,3] = sigma_acc_drift**2
print(Q, Q.shape)









    



[[  1.60000000e-13   8.00000000e-11   2.00000000e-08   0.00000000e+00]
 [  8.00000000e-11   4.00000000e-08   1.00000000e-05   0.00000000e+00]
 [  2.00000000e-08   1.00000000e-05   2.50000000e-03   0.00000000e+00]
 [  0.00000000e+00   0.00000000e+00   0.00000000e+00   1.00000000e-08]] (4, 4)

Identity Matrix



In [742]:

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









    



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

V) TEST

Re-init State



In [743]:

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

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

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

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

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

Filter loop



In [744]:

    
for filterstep in range(m):    
    
    # ========================
    # Time Update (Prediction)
    # ========================
    
    # Project the state ahead
    x = A*x
    
    # Project the error covariance ahead
    P = A*P*A.T + Q    
 
    
    # =============================
    # Correction from accelerometer 
    # =============================
    
    # Compute the Kalman Gain
    S = H_acc*P*H_acc.T + R_acc
    K = (P*H_acc.T) * np.linalg.pinv(S)
    
    # Update the estimate via z
    Z = meas_acc[filterstep]
    y = Z - (H_acc*x)                            # Innovation or Residual
    x = x + (K*y)
    
    # Update the error covariance
    P = (I - (K*H_acc))*P

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

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

VI) PLOT

Position $z$



In [745]:

    
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[745]:





<matplotlib.legend.Legend at 0x7f24b077a438>

Detail view of position estimate



In [746]:

    
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[746]:





<matplotlib.legend.Legend at 0x7f24b06e8cf8>

Accelerometer Bias $\zeta$



In [747]:

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









    Out[747]:





<matplotlib.legend.Legend at 0x7f24b06572e8>

Altitude Covariance



In [748]:

    
plt.plot(Pz)









    Out[748]:





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

Vertical acceleration $\ddot z$



In [749]:

    
plt.plot(t, meas_acc - acc_bias, '.r', alpha=0.3)
plt.plot(t, ddzt, '.b', alpha=0.3)
plt.plot(t, acc_true, 'g', linewidth=4)
plt.ylim([-1, 1])
plt.legend(['unbiased measure', 'estimate', 'truth'])









    Out[749]:





<matplotlib.legend.Legend at 0x7f24b05a6588>



In [749]:



In [749]: