In [2]:

    

%pylab inline
import sympy as sp
sp.init_printing()


    

    




Populating the interactive namespace from numpy and matplotlib

In [8]:

    

dt        = sp.Symbol('dt')
psi_k1_k1 = sp.Symbol('\psi_{k-1 | k-1}') # roll prior
psi_k_k1  = sp.Symbol('\psi_{k   | k-1}') # roll predicted
psi_k_k   = sp.Symbol('\psi_{k   | k  }') # roll post
d_psi     = sp.Symbol('\dot{\psi}_{k-1}') # roll rate
psi_m     = sp.Symbol('\psi_m')           # mesured roll

b_psi_k1_k1  = sp.Symbol('b_{\dot{\psi}}_{k-1 | k-1}')   # roll rate bias
b_psi_k_k1   = sp.Symbol('b_{\dot{\psi}}_{k   | k-1}')   # roll rate bias
b_psi_k_k    = sp.Symbol('b_{\dot{\psi}}_{k   | k  }')   # roll rate bias


    

In [32]:

    

x_k1_k1 = sp.Matrix([ psi_k1_k1,
                      b_psi_k1_k1 ]) # prior
x_k_k1  = sp.Matrix([ psi_k_k1,
                      b_psi_k_k1  ]) # predicted
x_k_k   = sp.Matrix([ psi_k_k,
                      b_psi_k_k   ]) # post

x_k_k


    

    
Out[32]:





$$\left[\begin{matrix}\psi_{{k   | k  }}\\b_{{\dot{\psi}} {k   | k  }}\end{matrix}\right]$$

In [10]:

    

sigma = sp.Symbol('\sigma_{m}')

# Measurement noise covariance
R = sigma * sp.eye(1)

R


    

    
Out[10]:





$$\left[\begin{matrix}\sigma_{{m}}\end{matrix}\right]$$

In [11]:

    

z_k = sp.Matrix([ psi_m ])

z_k


    

    
Out[11]:





$$\left[\begin{matrix}\psi_{m}\end{matrix}\right]$$

In [12]:

    

u_k1    = sp.Matrix([ d_psi ])

u_k1


    

    
Out[12]:





$$\left[\begin{matrix}\dot{\psi}_{{k-1}}\end{matrix}\right]$$

In [15]:

    

P_k1_k1 = sp.Matrix([ [sp.Symbol('p_' + str(i) + str(j) + '_{k-1 | k-1}') for j in [0,1] ] for i in [0,1] ])
P_k_k1  = sp.Matrix([ [sp.Symbol('p_' + str(i) + str(j) + '_{k   | k-1}') for j in [0,1] ] for i in [0,1] ])
P_k_k   = sp.Matrix([ [sp.Symbol('p_' + str(i) + str(j) + '_{k   | k  }') for j in [0,1] ] for i in [0,1] ])

P_k_k1


    

    
Out[15]:





$$\left[\begin{matrix}p_{00 {k   | k-1}} & p_{01 {k   | k-1}}\\p_{10 {k   | k-1}} & p_{11 {k   | k-1}}\end{matrix}\right]$$

In [21]:

    

# Uncertainty on model
sigma_q = sp.Symbol('\sigma_q')

# Uncertainty on bias drift
sigma_b = sp.Symbol('\sigma_{b_{\dot{\phi}}}')

w = sp.Matrix([dt, 1])
Q = sigma_b * w * w.T
Q[0,0] += sigma_q

Q


    

    
Out[21]:





$$\left[\begin{matrix}\sigma_{q} + \sigma_{{b {\dot{\phi}}}} dt^{2} & \sigma_{{b {\dot{\phi}}}} dt\\\sigma_{{b {\dot{\phi}}}} dt & \sigma_{{b {\dot{\phi}}}}\end{matrix}\right]$$

In [34]:

    

def f(x, u, dt):
    return sp.Matrix([ x[0] + dt*x[1] + dt*u[0],
                       x[1] ])

x_k_k1 = f( x_k1_k1, u_k1, dt )

x_k_k1


    

    
Out[34]:





$$\left[\begin{matrix}\dot{\psi}_{{k-1}} dt + \psi_{{k-1 | k-1}} + b_{{\dot{\psi}} {k-1 | k-1}} dt\\b_{{\dot{\psi}} {k-1 | k-1}}\end{matrix}\right]$$

In [38]:

    

F = f( x_k1_k1, u_k1, dt ).jacobian( x_k1_k1 )

F


    

    
Out[38]:





$$\left[\begin{matrix}1 & dt\\0 & 1\end{matrix}\right]$$

In [39]:

    

P_k_k1 = F * P_k1_k1 * F.T + Q

P_k_k1


    

    
Out[39]:





$$\left[\begin{matrix}\sigma_{q} + \sigma_{{b {\dot{\phi}}}} dt^{2} + dt p_{10 {k-1 | k-1}} + dt \left(dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\right) + p_{00 {k-1 | k-1}} & \sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\\\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + p_{10 {k-1 | k-1}} & \sigma_{{b {\dot{\phi}}}} + p_{11 {k-1 | k-1}}\end{matrix}\right]$$

In [40]:

    

def h(x):
    return sp.Matrix([x[0]])

h(x_k_k1)


    

    
Out[40]:





$$\left[\begin{matrix}\dot{\psi}_{{k-1}} dt + \psi_{{k-1 | k-1}} + b_{{\dot{\psi}} {k-1 | k-1}} dt\end{matrix}\right]$$

In [41]:

    

H = h(x_k_k1).jacobian(x_k1_k1)

H


    

    
Out[41]:





$$\left[\begin{matrix}1 & dt\end{matrix}\right]$$

In [42]:

    

y = z_k - h(x_k_k1)

y


    

    
Out[42]:





$$\left[\begin{matrix}- \dot{\psi}_{{k-1}} dt + \psi_{m} - \psi_{{k-1 | k-1}} - b_{{\dot{\psi}} {k-1 | k-1}} dt\end{matrix}\right]$$

In [43]:

    

S = H * P_k_k1 * H.T + R

S


    

    
Out[43]:





$$\left[\begin{matrix}\sigma_{q} + \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + dt p_{10 {k-1 | k-1}} + dt \left(dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\right) + dt \left(\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + p_{10 {k-1 | k-1}}\right) + dt \left(\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + dt \left(\sigma_{{b {\dot{\phi}}}} + p_{11 {k-1 | k-1}}\right) + p_{01 {k-1 | k-1}}\right) + p_{00 {k-1 | k-1}}\end{matrix}\right]$$

In [44]:

    

K = P_k_k1 * H.T * S**(-1)

K


    

    
Out[44]:





$$\left[\begin{matrix}\frac{\sigma_{q} + \sigma_{{b {\dot{\phi}}}} dt^{2} + dt p_{10 {k-1 | k-1}} + dt \left(dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\right) + dt \left(\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\right) + p_{00 {k-1 | k-1}}}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}}\\\frac{\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + dt \left(\sigma_{{b {\dot{\phi}}}} + p_{11 {k-1 | k-1}}\right) + p_{10 {k-1 | k-1}}}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}}\end{matrix}\right]$$

In [49]:

    

P_k_k = (sp.eye(2) - K * H) * P_k_k1

P_k_k.simplify()

P_k_k


    

    
Out[49]:





$$\left[\begin{matrix}\frac{1}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}} \left(\sigma_{q} \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{q} \sigma_{{m}} + \sigma_{q} dt^{2} p_{11 {k-1 | k-1}} + \sigma_{{b {\dot{\phi}}}} \sigma_{{m}} dt^{2} + \sigma_{{b {\dot{\phi}}}} dt^{2} p_{00 {k-1 | k-1}} + \sigma_{{m}} dt^{2} p_{11 {k-1 | k-1}} + \sigma_{{m}} dt p_{01 {k-1 | k-1}} + \sigma_{{m}} dt p_{10 {k-1 | k-1}} + \sigma_{{m}} p_{00 {k-1 | k-1}} + dt^{2} p_{00 {k-1 | k-1}} p_{11 {k-1 | k-1}} - dt^{2} p_{01 {k-1 | k-1}} p_{10 {k-1 | k-1}}\right) & \frac{- \sigma_{q} \sigma_{{b {\dot{\phi}}}} dt - \sigma_{q} dt p_{11 {k-1 | k-1}} + \sigma_{{b {\dot{\phi}}}} \sigma_{{m}} dt - \sigma_{{b {\dot{\phi}}}} dt p_{00 {k-1 | k-1}} + \sigma_{{m}} dt p_{11 {k-1 | k-1}} + \sigma_{{m}} p_{01 {k-1 | k-1}} - dt p_{00 {k-1 | k-1}} p_{11 {k-1 | k-1}} + dt p_{01 {k-1 | k-1}} p_{10 {k-1 | k-1}}}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}}\\\frac{- \sigma_{q} \sigma_{{b {\dot{\phi}}}} dt - \sigma_{q} dt p_{11 {k-1 | k-1}} + \sigma_{{b {\dot{\phi}}}} \sigma_{{m}} dt - \sigma_{{b {\dot{\phi}}}} dt p_{00 {k-1 | k-1}} + \sigma_{{m}} dt p_{11 {k-1 | k-1}} + \sigma_{{m}} p_{10 {k-1 | k-1}} - dt p_{00 {k-1 | k-1}} p_{11 {k-1 | k-1}} + dt p_{01 {k-1 | k-1}} p_{10 {k-1 | k-1}}}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}} & \frac{\sigma_{q} \sigma_{{b {\dot{\phi}}}} + \sigma_{q} p_{11 {k-1 | k-1}} + \sigma_{{b {\dot{\phi}}}} \sigma_{{m}} + \sigma_{{b {\dot{\phi}}}} p_{00 {k-1 | k-1}} + \sigma_{{m}} p_{11 {k-1 | k-1}} + p_{00 {k-1 | k-1}} p_{11 {k-1 | k-1}} - p_{01 {k-1 | k-1}} p_{10 {k-1 | k-1}}}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}}\end{matrix}\right]$$

In [53]:

    

x_k_k = x_k_k1 + K * y

x_k_k.simplify()

x_k_k


    

    
Out[53]:





$$\left[\begin{matrix}\frac{1}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}} \left(\left(\dot{\psi}_{{k-1}} dt + \psi_{{k-1 | k-1}} + b_{{\dot{\psi}} {k-1 | k-1}} dt\right) \left(\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}\right) - \left(\dot{\psi}_{{k-1}} dt - \psi_{m} + \psi_{{k-1 | k-1}} + b_{{\dot{\psi}} {k-1 | k-1}} dt\right) \left(\sigma_{q} + \sigma_{{b {\dot{\phi}}}} dt^{2} + dt p_{10 {k-1 | k-1}} + dt \left(dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\right) + dt \left(\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + p_{01 {k-1 | k-1}}\right) + p_{00 {k-1 | k-1}}\right)\right)\\\frac{1}{\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}} \left(b_{{\dot{\psi}} {k-1 | k-1}} \left(\sigma_{q} + 4 \sigma_{{b {\dot{\phi}}}} dt^{2} + \sigma_{{m}} + 4 dt^{2} p_{11 {k-1 | k-1}} + 2 dt p_{01 {k-1 | k-1}} + 2 dt p_{10 {k-1 | k-1}} + p_{00 {k-1 | k-1}}\right) - \left(\dot{\psi}_{{k-1}} dt - \psi_{m} + \psi_{{k-1 | k-1}} + b_{{\dot{\psi}} {k-1 | k-1}} dt\right) \left(\sigma_{{b {\dot{\phi}}}} dt + dt p_{11 {k-1 | k-1}} + dt \left(\sigma_{{b {\dot{\phi}}}} + p_{11 {k-1 | k-1}}\right) + p_{10 {k-1 | k-1}}\right)\right)\end{matrix}\right]$$