

In [3]:

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









    



Populating the interactive namespace from numpy and matplotlib

Symbolic variables



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

tau       = sp.Symbol('\\tau')            # filter response time

Filter equations

State Prediction



In [11]:

    
psi_k_k1 = psi_k1_k1 + d_psi * dt 

psi_k_k1









    Out[11]:





$$\dot{\psi}_{{k-1}} dt + \psi_{{k-1 | k-1}}$$

alpha



In [9]:

    
alpha = tau / (tau + dt)

alpha









    Out[9]:





$$\frac{\tau}{\tau + dt}$$

State correction



In [12]:

    
psi_k_k = alpha * psi_k_k1 + (1-alpha) * psi_m

psi_k_k









    Out[12]:





$$\psi_{m} \left(- \frac{\tau}{\tau + dt} + 1\right) + \frac{\tau}{\tau + dt} \left(\dot{\psi}_{{k-1}} dt + \psi_{{k-1 | k-1}}\right)$$



In [ ]: