

In [2]:

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

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

t = np.arange(m) * dt

freq = 0.1      # Hz
amplitude = 0.5 # meter

alt_true = 405 + amplitude * np.cos(2 * np.pi * freq * t)
height_true = 5 + amplitude * np.cos(2 * np.pi * freq * t)
vel_true = - amplitude * (2 * np.pi * freq) * np.sin(2 * np.pi * freq * t)
acc_true = - amplitude * (2 * np.pi * freq)**2 * np.cos(2 * np.pi * freq * t)

plt.plot(t, height_true)
plt.plot(t, vel_true)
plt.plot(t, acc_true)
plt.legend(['elevation', 'velocity', 'acceleration'], loc='best')
plt.xlabel('time')









    Out[3]:





<matplotlib.text.Text at 0x7fa2f441c320>

II) MEASUREMENTS

Sonar



In [4]:

    
sonar_sampling_period = 1 / 10.0  # sonar reading at 10Hz

# Sonar noise
sigma_sonar_true = 0.05  # in meters


meas_sonar = height_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, height_true)
plt.legend(['Sonar measure', 'Elevation (true)'])
plt.title("Sonar measurement")
plt.xlabel('time (s)')
plt.ylabel('alt (m)')









    Out[4]:





<matplotlib.text.Text at 0x7fa2bf668710>

Baro



In [5]:

    
baro_sampling_period = 1 / 10.0  # baro reading at 10Hz

# Baro noise
sigma_baro_true = 2.0  # in meters

meas_baro = alt_true[::(baro_sampling_period/dt)] + sigma_baro_true * np.random.randn(m // (baro_sampling_period/dt))
t_meas_baro = t[::(baro_sampling_period/dt)]

plt.plot(t_meas_baro, meas_baro, 'or')
plt.plot(t, alt_true)
plt.title("Baro measurement")
plt.xlabel('time (s)')
plt.ylabel('alt (m)')









    Out[5]:





<matplotlib.text.Text at 0x7fa2bf5d4278>

GPS



In [6]:

    
gps_sampling_period = 1 / 1.0  # gps reading at 1Hz

# GPS noise
sigma_gps_true = 5.0  # in meters

meas_gps = alt_true[::(gps_sampling_period/dt)] + sigma_gps_true * np.random.randn(m // (gps_sampling_period/dt))
t_meas_gps = t[::(gps_sampling_period/dt)]

plt.plot(t_meas_gps, meas_gps, 'or')
plt.plot(t, alt_true)
plt.title("GPS measurement")
plt.xlabel('time (s)')
plt.ylabel('alt (m)')









    Out[6]:





<matplotlib.text.Text at 0x7fa2bf5b3320>

GPS velocity



In [7]:

    
gpsvel_sampling_period = 1 / 1.0  # gps reading at 1Hz

# GPS noise
sigma_gpsvel_true = 10.0  # in meters/s

meas_gpsvel = vel_true[::(gps_sampling_period/dt)] + sigma_gpsvel_true * np.random.randn(m // (gps_sampling_period/dt))
t_meas_gps = t[::(gps_sampling_period/dt)]

plt.plot(t_meas_gps, meas_gpsvel, 'or')
plt.plot(t, vel_true)
plt.title("GPS velocity measurement")
plt.xlabel('time (s)')
plt.ylabel('vel (m/s)')









    Out[7]:





<matplotlib.text.Text at 0x7fa2bf538390>

Acceleration



In [8]:

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





<matplotlib.text.Text at 0x7fa2bf5186a0>

III) PROBLEM FORMULATION

State vector

$$x_{k} = \left[ \matrix{ z \\ h \\ \dot z \\ \zeta } \right] = \matrix{ \text{Altitude} \\ \text{Height above ground} \\ \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 & 0 & \Delta_t & \frac{1}{2} \Delta t^2 \\ 0 & 1 & \Delta t & \frac{1}{2} \Delta t^2 \\ 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 1 } \right] \cdot \left[ \matrix{ z \\ h \\ \dot z \\ \zeta } \right] + \left[ \matrix{ \frac{1}{2} \Delta t^2 \\ \frac{1}{2} \Delta t^2 \\ \Delta t \\ 0 } \right] \cdot \left[ \matrix{ \ddot z } \right] $$

Measurement

$$ y = H \cdot x $$$$ \left[ \matrix{ y_{sonar} \\ y_{baro} \\ y_{gps} \\ y_{gpsvel} } \right] = \left[ \matrix{ 0 & 1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 } \right] \cdot \left[ \matrix{ z \\ h \\ \dot z \\ \zeta } \right] $$

Measures are done separately according to the refresh rate of each sensor

We measure the height from sonar

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

We measure the altitude from barometer

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

We measure the altitude from gps

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

We measure the velocity from gps

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

IV) IMPLEMENTATION

Initial state $x_0$



In [9]:

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

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

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

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









    



[[  1.00000000e+00   0.00000000e+00   4.00000000e-03   8.00000000e-06]
 [  0.00000000e+00   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]] (4, 4)

Disturbance Control Matrix $B$



In [12]:

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









    



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

Measurement Matrix $H$



In [13]:

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

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

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

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









    



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

Measurement noise covariance $R$



In [14]:

    
# sonar
sigma_sonar = sigma_sonar_true # sonar noise

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

# baro
sigma_baro = sigma_baro_true # sonar noise

R_baro = np.matrix([[sigma_baro**2]])
print(R_baro, R_baro.shape)

# gps
sigma_gps = sigma_gps_true # sonar noise

R_gps = np.matrix([[sigma_gps**2]])
print(R_gps, R_gps.shape)

# gpsvel
sigma_gpsvel = sigma_gpsvel_true # sonar noise

R_gpsvel = np.matrix([[sigma_gpsvel**2]])
print(R_gpsvel, R_gpsvel.shape)









    



[[ 0.0025]] (1, 1)
[[ 4.]] (1, 1)
[[ 25.]] (1, 1)
[[ 100.]] (1, 1)

Process noise covariance $Q$



In [15]:

    
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], [0.5*dts**2], [dts], [1.0]])
Qs*Qs.T*s1**2









    Out[15]:





$$\left[\begin{matrix}0.25 \Delta t^{4} \sigma_{1}^{2} & 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.25 \Delta t^{4} \sigma_{1}^{2} & 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.5 \Delta t^{3} \sigma_{1}^{2} & 0.5 \Delta t^{3} \sigma_{1}^{2} & \Delta t^{2} \sigma_{1}^{2} & 1.0 \Delta t \sigma_{1}^{2}\\0.5 \Delta t^{2} \sigma_{1}^{2} & 0.5 \Delta t^{2} \sigma_{1}^{2} & 1.0 \Delta t \sigma_{1}^{2} & 1.0 \sigma_{1}^{2}\end{matrix}\right]$$



In [16]:

    
sigma_acc_drift = 0.0001

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

print(Q, Q.shape)









    



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

Identity Matrix



In [17]:

    
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)

Input



In [18]:

    
u = meas_acc
print(u, u.shape)









    



[ 1.43758068  1.31730183  1.34765267 ...,  1.03418452  1.54351946
  1.19880486] (10000,)

V) TEST

Filter loop



In [21]:

    
# Re init state
# State
x[0] = 300.0
x[1] = 5.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 = []
ht = []
dzt= []
zetat=[]

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

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

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)
    # ===============================
    
    # Sonar (only at the beginning, ex take off)
    if filterstep%25 == 0 and (filterstep <2000 or filterstep>9000):    
        # Compute the Kalman Gain
        S_sonar = H_sonar*P*H_sonar.T + R_sonar
        K_sonar = (P*H_sonar.T) * np.linalg.pinv(S_sonar)
    
        # Update the estimate via z
        Z_sonar = meas_sonar[filterstep//25]
        y_sonar = Z_sonar - (H_sonar*x)                            # Innovation or Residual
        x = x + (K_sonar*y_sonar)
    
        # Update the error covariance
        P = (I - (K_sonar*H_sonar))*P
        
    # Baro
    if filterstep%25 == 0:    
        # Compute the Kalman Gain
        S_baro = H_baro*P*H_baro.T + R_baro
        K_baro = (P*H_baro.T) * np.linalg.pinv(S_baro)
    
        # Update the estimate via z
        Z_baro = meas_baro[filterstep//25]
        y_baro = Z_baro - (H_baro*x)                            # Innovation or Residual
        x = x + (K_baro*y_baro)
    
        # Update the error covariance
        P = (I - (K_baro*H_baro))*P

    # GPS
    if filterstep%250 == 0:    
        # Compute the Kalman Gain
        S_gps = H_gps*P*H_gps.T + R_gps
        K_gps = (P*H_gps.T) * np.linalg.pinv(S_gps)
    
        # Update the estimate via z
        Z_gps = meas_gps[filterstep//250]
        y_gps = Z_gps - (H_gps*x)                            # Innovation or Residual
        x = x + (K_gps*y_gps)
    
        # Update the error covariance
        P = (I - (K_gps*H_gps))*P

    # GPSvel
    if filterstep%250 == 0:    
        # Compute the Kalman Gain
        S_gpsvel = H_gpsvel*P*H_gpsvel.T + R_gpsvel
        K_gpsvel = (P*H_gpsvel.T) * np.linalg.pinv(S_gpsvel)
    
        # Update the estimate via z
        Z_gpsvel = meas_gpsvel[filterstep//250]
        y_gpsvel = Z_gpsvel - (H_gpsvel*x)                            # Innovation or Residual
        x = x + (K_gpsvel*y_gpsvel)
    
        # Update the error covariance
        P = (I - (K_gpsvel*H_gpsvel))*P
        
        
    # ========================
    # Save states for Plotting
    # ========================
    zt.append(float(x[0]))
    ht.append(float(x[1]))
    dzt.append(float(x[2]))
    zetat.append(float(x[3]))

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

VI) PLOT

Altitude $z$



In [22]:

    
plt.figure(figsize=(17,15))

plt.subplot(321)
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_baro, meas_baro, '.r')
plt.plot(t_meas_gps, meas_gps, 'ok')
plt.plot([t[2000], t[2000]], [-1000, 1000], '--k')
plt.plot([t[9000], t[9000]], [-1000, 1000], '--k')
#plt.ylim([1.7, 2.3])
plt.ylim([405 - 50 * amplitude, 405 + 30 * amplitude])
plt.legend(['estimate', 'true altitude', 'baro reading', 'gps reading', 'sonar switched off/on'], loc='lower right')
plt.title('Altitude')

plt.subplot(322)
plt.plot(t, ht, color='b')
plt.fill_between(t, np.array(ht) - 10* np.array(Ph), np.array(ht) + 10*np.array(Ph), alpha=0.2, color='b')
plt.plot(t, height_true, 'g')
plt.plot(t_meas_sonar, meas_sonar, '.r')
plt.plot([t[2000], t[2000]], [-1000, 1000], '--k')
plt.plot([t[9000], t[9000]], [-1000, 1000], '--k')
#plt.ylim([1.7, 2.3])
# plt.ylim([5 - 1.5 * amplitude, 5 + 1.5 * amplitude])
plt.ylim([5 - 10 * amplitude, 5 + 10 * amplitude])
plt.legend(['estimate', 'true height above ground', 'sonar reading', 'sonar switched off/on'])
plt.title('Height')

plt.subplot(323)
plt.plot(t, dzt, color='b')
plt.fill_between(t, np.array(dzt) - 10* np.array(Pdz), np.array(dzt) + 10*np.array(Pdz), alpha=0.2, color='b')
plt.plot(t, vel_true, 'g')
plt.plot(t_meas_gps, meas_gpsvel, 'ok')
plt.plot([t[2000], t[2000]], [-1000, 1000], '--k')
plt.plot([t[9000], t[9000]], [-1000, 1000], '--k')
#plt.ylim([1.7, 2.3])
plt.ylim([0 - 10.0 * amplitude,  + 10.0 * amplitude])
plt.legend(['estimate', 'true velocity', 'gps_vel reading', 'sonar switched off/on'])
plt.title('Velocity')

plt.subplot(324)
plt.plot(t, zetat, color='b')
plt.fill_between(t, np.array(zetat) - 10* np.array(Pzeta), np.array(zetat) + 10*np.array(Pzeta), alpha=0.2, color='b')
plt.plot(t, -acc_bias * np.ones_like(t), 'g')
plt.plot([t[2000], t[2000]], [-1000, 1000], '--k')
plt.plot([t[9000], t[9000]], [-1000, 1000], '--k')
plt.ylim([-2.0, 1.0])
# plt.ylim([0 - 2.0 * amplitude,  + 2.0 * amplitude])
plt.legend(['estimate', 'true bias', 'sonar switched off/on'])
plt.title('Acc bias')

plt.subplot(325)
plt.plot(t, Pz)
plt.plot(t, Ph)
plt.plot(t, Pdz)
plt.ylim([0, 1.0])
plt.plot([t[2000], t[2000]], [-1000, 1000], '--k')
plt.plot([t[9000], t[9000]], [-1000, 1000], '--k')
plt.legend(['Altitude', 'Height', 'Velocity', 'sonar switched off/on'])
plt.title('Incertitudes')









    Out[22]:





<matplotlib.text.Text at 0x7fa2bd51e940>



In [ ]: