```python
from __future__ import division, print_function
%matplotlib inline
```


```python
import sympy
from sympy import symbols, sin, cos, sqrt, diag
from sympy import Matrix, BlockMatrix, eye, ones, zeros
from sympy.physics.mechanics import *
from IPython.display import display
sympy.init_printing(use_latex='mathjax')

import numpy as np


```

# Quaternion Math


```python
def expq(n):
    n *= 0.5
    nNorm = n.norm()
    qn = Matrix([cos(nNorm),n/nNorm*sin(nNorm)])
    return qn

def quat2dcm(q):
    """
    Convert quaternion to DCM
    """
    
    # Extract components
    w = q[0]
    x = q[1]
    y = q[2]
    z = q[3]
    
    # Reduce repeated calculations
    ww = w*w
    xx = x*x
    yy = y*y
    zz = z*z  
    wx = w*x
    wy = w*y
    wz = w*z
    xy = x*y
    xz = x*z
    yz = y*z
    
    # Build Direction Cosine Matrix (DCM)   
    dcm = Matrix([
        [ww + xx - yy - zz,       2*(xy - wz),       2*(xz + wy)],
        [      2*(xy + wz), ww - xx + yy - zz,       2*(yz - wx)],
        [      2*(xz - wy),       2*(yz + wx), ww - xx - yy + zz]
    ])
    return dcm

def dcm2quat(dcm):
    """
    Determine quaternion corresponding to dcm using
    the stanley method. 
    
    Flips sign to always return shortest path quaterion
    so w >= 0
    
    Converts the 3x3 DCM into the quaterion where the 
    first component is the real part
    """
    
    tr = Matrix.trace(dcm)
    
    w = 0.25*(1+tr)
    x = 0.25*(1+2*dcm[0,0]-tr)
    y = 0.25*(1+2*dcm[1,1]-tr)
    z = 0.25*(1+2*dcm[2,2]-tr)
    
    #kMax = np.argmax([w,x,y,z])
    
    kMax = 0
    
    if kMax == 0:
        w = sqrt(w)
        x = 0.25*(dcm[1,2]-dcm[2,1])/w
        y = 0.25*(dcm[2,0]-dcm[0,2])/w
        z = 0.25*(dcm[0,1]-dcm[1,0])/w
    
    elif kMax == 1:
        x = sqrt(x)
        w = 0.25*(dcm[1,2]-dcm[2,1])/x
        if w<0:
            x = -x
            w = -w
        y = 0.25*(dcm[0,1]+dcm[1,0])/x
        z = 0.25*(dcm[2,0]+dcm[0,2])/x
        
    elif kMax == 2:
        y = sqrt(y)
        w = 0.25*(dcm[2,0]-dcm[0,2])/y
        if w<0:
            y = -y
            w = -w
        x = 0.25*(dcm[0,1]+dcm[1,0])/y
        z = 0.25*(dcm[1,2]+dcm[2,1])/y
        
    elif kMax == 3:
        z = sqrt(z)
        w = 0.25*(dcm[0,1]-dcm[1,0])/z
        if w<0:
            z = -z
            w = -w
        x = 0.25*(dcm[2,0]+dcm[0,2])/z
        y = 0.25*(dcm[1,2]+dcm[2,1])/z
        
    q = Matrix([w,x,y,z])
    
    return q

def skew3(v):
    vx,vy,vz = v
    out = Matrix([[  0, -vz,   vy],
                  [ vz,   0,  -vx],
                  [-vy,  vx,    0]])
    return out

def skew4Left(v):
    if len(v)==3:
        v = Matrix.vstack(zeros(1),v)
    w,x,y,z = v
    out = Matrix([
            [w, -x, -y, -z],
            [x,  w, -z,  y],
            [y,  z,  w, -x],
            [z, -y,  x,  w],
        ])        
    return out

def skew4Right(v):
    if len(v)==3:
        v = Matrix.vstack(zeros(1),v)
    w,x,y,z = v
    out = Matrix([
            [w, -x, -y, -z],
            [x,  w,  z, -y],
            [y, -z,  w,  x],
            [z,  y, -x,  w],
        ])      
    return out


def quatConj(q):
    q_out = Matrix(q[:])
    q_out = q_out.T*sympy.diag(1,-1,-1,-1)
    q_out = q_out.T

    return q_out

def qRot(q,v):
    qPrime = quatConj(q)
    v = Matrix.vstack(zeros(1),v)
    vout = skew4Left(q)*skew4Right(qPrime)*v
    return Matrix(vout[1:])


def dRotdq(q,v):
    qw,qx,qy,qz = q
    vx,vy,vz = v

    dRdq = Matrix([
        [2*qw*vx + 2*qy*vz - 2*qz*vy,  2*qx*vx + 2*qy*vy + 2*qz*vz,  2*qw*vz + 2*qx*vy - 2*qy*vx, -2*qw*vy + 2*qx*vz - 2*qz*vx],
        [2*qw*vy - 2*qx*vz + 2*qz*vx, -2*qw*vz - 2*qx*vy + 2*qy*vx,  2*qx*vx + 2*qy*vy + 2*qz*vz,  2*qw*vx + 2*qy*vz - 2*qz*vy],
        [2*qw*vz + 2*qx*vy - 2*qy*vx,  2*qw*vy - 2*qx*vz + 2*qz*vx, -2*qw*vx - 2*qy*vz + 2*qz*vy,  2*qx*vx + 2*qy*vy + 2*qz*vz]
    ])

    return dRdq

def dVdq(q,v):
    qw,qx,qy,qz = q
    vx,vy,vz = v
    
    dv = Matrix([
        [ 2*vx*qw + 2*vy*qz - 2*vz*qy, 2*vx*qx + 2*vy*qy + 2*vz*qz, -2*vx*qy + 2*vy*qx - 2*vz*qw, -2*vx*qz + 2*vy*qw + 2*vz*qx],
        [-2*vx*qz + 2*vy*qw + 2*vz*qx, 2*vx*qy - 2*vy*qx + 2*vz*qw,  2*vx*qx + 2*vy*qy + 2*vz*qz, -2*vx*qw - 2*vy*qz + 2*vz*qy],
        [ 2*vx*qy - 2*vy*qx + 2*vz*qw, 2*vx*qz - 2*vy*qw - 2*vz*qx,  2*vx*qw + 2*vy*qz - 2*vz*qy,  2*vx*qx + 2*vy*qy + 2*vz*qz],
    ])
    
    return dv

```

# Setup


```python
# Time step
t = symbols('T')

# States
qw,qx,qy,qz = symbols('q_w q_x q_y q_z')
rx,ry,rz = symbols('r_x r_y r_z') 
vx,vy,vz = symbols('v_x v_y v_z')
abx,aby,abz = symbols('a_bx a_by a_bz')
gbx,gby,gbz = symbols('g_bx g_by g_bz')

r_L = Matrix([rx,ry,rz])
v_L = Matrix([vx,vy,vz])
q_toLfromB = Matrix([qw,qx,qy,qz])
gyroBias = Matrix([gbx,gby,gbz])
accelBias = Matrix([abx,aby,abz])

# Measurements
ax,ay,az = symbols('a_x a_y a_z') # measured
mx,my,mz = symbols('m_x m_y m_z') # measured 
wx,wy,wz = symbols('w_x w_y w_z') # measured

accelMeas = Matrix([ax,ay,az])
magMeas = Matrix([mx,my,mz]) 
wMeas = Matrix([wx,wy,wz])

# Nav vectors
g = symbols('g')
bx,by,bz = symbols('b_x b_y b_z') 

g_L = Matrix([0,0,g])
B_L = Matrix([mx,0,0])

# Measurment Uncertanties
eGyro, eAccel, eMag, eGPS = symbols('eGyro eAccel eMag eGPS')
ewx,ewy,ewz = symbols('e_wx e_wy e_wz')
eax,eay,eaz = symbols('e_ax e_ay e_az')
emx,emy,emz = symbols('e_mx e_my e_mz')

ew = Matrix([ewx,ewy,ewz])
ea = Matrix([eax,eay,eaz])
em = Matrix([emx,emy,emz])

eVec = Matrix([
    ew,
    ea,
    em,
])

esub0 = {ewx:0,ewy:0,ewz:0,
         eax:0,eay:0,eaz:0,
         emx:0,emy:0,emz:0}

#erx,ery,erz = symbols('e_wx e_wy w_wz')
#evx,evy,evz = symbols('e_ax e_ay w_az')
#ewx,ewy,ewz = symbols('e_wx e_wy w_wz')

#er = Matrix([erx,ery,erz])
#ev = Matrix([evx,evy,evz])
#ew = Matrix([ewx,ewy,ewz])

#eVec = Matrix([
#    er,
#    ev,
#    ew,
#])

#esub0 = {erx:0,ery:0,erz:0,
#         evx:0,evy:0,evz:0,
#         ewx:0,ewy:0,ewz:0}

wax,way,waz = symbols('w_ax w_ay w_az') 
wa = symbols('w_a')
ww = symbols('w_w')
waB = symbols('w_aB')
wgB = symbols('w_gB')

```

# Validate quaternion math


```python
print('dcm')
display( quat2dcm(q_toLfromB) )
print('[q]L')
display( skew4Left(q_toLfromB) )
print('[q]R')
display( skew4Right(q_toLfromB) )
print('[wb]R')
display( skew4Right(wMeas) )
r_B = Matrix([rx,ry,rz])
print('d/dq R*r_B')
display(dRotdq(q_toLfromB, r_B))


```

    dcm



$\displaystyle \left[\begin{matrix}q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2} & - 2 q_{w} q_{z} + 2 q_{x} q_{y} & 2 q_{w} q_{y} + 2 q_{x} q_{z}\\2 q_{w} q_{z} + 2 q_{x} q_{y} & q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2} & - 2 q_{w} q_{x} + 2 q_{y} q_{z}\\- 2 q_{w} q_{y} + 2 q_{x} q_{z} & 2 q_{w} q_{x} + 2 q_{y} q_{z} & q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\end{matrix}\right]$


    [q]L



$\displaystyle \left[\begin{matrix}q_{w} & - q_{x} & - q_{y} & - q_{z}\\q_{x} & q_{w} & - q_{z} & q_{y}\\q_{y} & q_{z} & q_{w} & - q_{x}\\q_{z} & - q_{y} & q_{x} & q_{w}\end{matrix}\right]$


    [q]R



$\displaystyle \left[\begin{matrix}q_{w} & - q_{x} & - q_{y} & - q_{z}\\q_{x} & q_{w} & q_{z} & - q_{y}\\q_{y} & - q_{z} & q_{w} & q_{x}\\q_{z} & q_{y} & - q_{x} & q_{w}\end{matrix}\right]$


    [wb]R



$\displaystyle \left[\begin{matrix}0 & - w_{x} & - w_{y} & - w_{z}\\w_{x} & 0 & w_{z} & - w_{y}\\w_{y} & - w_{z} & 0 & w_{x}\\w_{z} & w_{y} & - w_{x} & 0\end{matrix}\right]$


    d/dq R*r_B



$\displaystyle \left[\begin{matrix}2 q_{w} r_{x} + 2 q_{y} r_{z} - 2 q_{z} r_{y} & 2 q_{x} r_{x} + 2 q_{y} r_{y} + 2 q_{z} r_{z} & 2 q_{w} r_{z} + 2 q_{x} r_{y} - 2 q_{y} r_{x} & - 2 q_{w} r_{y} + 2 q_{x} r_{z} - 2 q_{z} r_{x}\\2 q_{w} r_{y} - 2 q_{x} r_{z} + 2 q_{z} r_{x} & - 2 q_{w} r_{z} - 2 q_{x} r_{y} + 2 q_{y} r_{x} & 2 q_{x} r_{x} + 2 q_{y} r_{y} + 2 q_{z} r_{z} & 2 q_{w} r_{x} + 2 q_{y} r_{z} - 2 q_{z} r_{y}\\2 q_{w} r_{z} + 2 q_{x} r_{y} - 2 q_{y} r_{x} & 2 q_{w} r_{y} - 2 q_{x} r_{z} + 2 q_{z} r_{x} & - 2 q_{w} r_{x} - 2 q_{y} r_{z} + 2 q_{z} r_{y} & 2 q_{x} r_{x} + 2 q_{y} r_{y} + 2 q_{z} r_{z}\end{matrix}\right]$


# Extended Kalman Filter Equations

## State Equations

For the Linear Kalman filter we have the following for the process and measurment models:

$$\begin{aligned}\dot{\mathbf x} &= \mathbf{Ax} + w_x\\
\mathbf z &= \mathbf{Hx} + w_z
\end{aligned}$$

Where $\mathbf A$ is the systems dynamic matrix. Using the state space methodsthese equations can be transformed into:
$$\begin{aligned}\bar{\mathbf x} &= \mathbf{Fx} \\
\mathbf z &= \mathbf{Hx}
\end{aligned}$$

For the nonlinear model the linear expression $\mathbf{Fx} + \mathbf{Bu}$ is replaced by a nonlinear function $f(\mathbf x, \mathbf u)$, and the linear expression $\mathbf{Hx}$ is replaced by a nonlinear function $h(\mathbf x)$:

$$\begin{aligned}\dot{\mathbf x} &= f(\mathbf x, \mathbf u) + w_x\\
\mathbf z &= h(\mathbf x) + w_z
\end{aligned}$$

The EKF doesn't alter the Kalman Filter's linear equations. Instead, it *linearizes* the nonlinear equations at the point of the current estimate. The system is linearized by using the jacobian.

$$
\begin{aligned}
\mathbf F 
&= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\
\mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} 
\end{aligned}
$$

 
## Summary of Linear and Nonlinear Kalman Filter Equations


$$\begin{array}{l|l}
\text{Linear Kalman filter} & \text{EKF} \\
\hline 
& \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\
\mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)}  \\
\mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q  & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\
\hline
& \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\
\textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\
\mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\
\mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\
\mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}}
\end{array}$$





## Measurment Model 

The measurment model jacobian should be (N_measurments, N_states)

$$
\mathbf H = \frac{\partial{h(\bar{\mathbf x})}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} = 
\begin{bmatrix}
\frac{\partial h_1}{\partial x_1} & \frac{\partial h_1}{\partial x_2} &\dots \\
\frac{\partial h_2}{\partial x_1} & \frac{\partial h_2}{\partial x_2} &\dots \\
\vdots & \vdots
\end{bmatrix}
$$



# Quadcopter Model

Here we mode the system dynamics using a constant acceleration model where the inertial acceleration acts as the input to the inertial position and velocity functions. We also represent the body attitude using the quaternion representation.

The measurments consist of an imu+mag and gps. Together this provides body acceleration, body rates, estimated gravity vector, estimated north vector, inertial position (and possibly inertial velocity).

For now, the accel and gyro bias states will be excluded.

## State and Measurment Models

Constant acceleration model with acceleration as input and position sensor (GPS) measurements



```python
# State vector
X = Matrix([
    r_L,
    v_L,
    q_toLfromB,
])

# State transition model (const accel & angAccel)
a_B = accelMeas
a_L  = qRot(q_toLfromB, a_B) - g_L

fx_r = r_L + v_L*t + 1/2*a_L*t**2
fx_v = v_L + a_L*t

wb = wMeas
fx_q = (eye(4) + t/2*skew4Right(wb))*q_toLfromB

#expq_wt = expq(wMeas*t)
#fx_q = skew4Left(q_toLfromB) * expq_wt

fx = Matrix([
    fx_r,
    fx_v,
    fx_q
])

# Measurment Model (estimates measurment from states)
hx_gps = r_L # used for position
hx_accel = -qRot(quatConj(q_toLfromB), g_L) # used for attitude
hx_mag   =  qRot(quatConj(q_toLfromB), B_L) # used for attitude

hx = Matrix([
    hx_gps,
    hx_accel,
    hx_mag,
])


# Display
print('State vector (X)')
display(X,X.shape)
print('State transition model (fx)')
display(fx,fx.shape)
print('Measurment function (hx)')
display(hx,hx.shape)

```

    State vector (X)



$\displaystyle \left[\begin{matrix}r_{x}\\r_{y}\\r_{z}\\v_{x}\\v_{y}\\v_{z}\\q_{w}\\q_{x}\\q_{y}\\q_{z}\end{matrix}\right]$



$\displaystyle \left( 10, \  1\right)$


    State transition model (fx)



$\displaystyle \left[\begin{matrix}T^{2} \left(0.5 a_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + 0.5 a_{y} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + 0.5 a_{z} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + T v_{x} + r_{x}\\T^{2} \left(0.5 a_{x} \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + 0.5 a_{y} \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + 0.5 a_{z} \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + T v_{y} + r_{y}\\T^{2} \left(0.5 a_{x} \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + 0.5 a_{y} \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + 0.5 a_{z} \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) - 0.5 g\right) + T v_{z} + r_{z}\\T \left(a_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + a_{y} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + a_{z} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + v_{x}\\T \left(a_{x} \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + a_{y} \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + a_{z} \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + v_{y}\\T \left(a_{x} \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + a_{y} \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + a_{z} \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) - g\right) + v_{z}\\- \frac{T q_{x} w_{x}}{2} - \frac{T q_{y} w_{y}}{2} - \frac{T q_{z} w_{z}}{2} + q_{w}\\\frac{T q_{w} w_{x}}{2} + \frac{T q_{y} w_{z}}{2} - \frac{T q_{z} w_{y}}{2} + q_{x}\\\frac{T q_{w} w_{y}}{2} - \frac{T q_{x} w_{z}}{2} + \frac{T q_{z} w_{x}}{2} + q_{y}\\\frac{T q_{w} w_{z}}{2} + \frac{T q_{x} w_{y}}{2} - \frac{T q_{y} w_{x}}{2} + q_{z}\end{matrix}\right]$



$\displaystyle \left( 10, \  1\right)$


    Measurment function (hx)



$\displaystyle \left[\begin{matrix}r_{x}\\r_{y}\\r_{z}\\- g \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\\- g \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\\- g \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\\m_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right)\\m_{x} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right)\\m_{x} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\end{matrix}\right]$



$\displaystyle \left( 9, \  1\right)$


## State and Measurment Jacobians

Determine the jacobian of the state and measurment models to implement the EKF for nonlinear systems.



```python
# State model jacobian
dfdx = fx.jacobian(X)

# Measurment model jacobian
dhdx = hx.jacobian(X)

# G (dfde)
#dfde = fx.jacobian(eVec)

# Display
print('State model jacobian (dfdx)')
display(dfdx,dfdx.shape)
print('Measurment model jacobian (dhdx)')
display(dhdx,dhdx.shape)

```

    State model jacobian (dfdx)



$\displaystyle \left[\begin{matrix}1 & 0 & 0 & T & 0 & 0 & T^{2} \left(1.0 a_{x} q_{w} - 1.0 a_{y} q_{z} + 1.0 a_{z} q_{y}\right) & T^{2} \left(1.0 a_{x} q_{x} + 1.0 a_{y} q_{y} + 1.0 a_{z} q_{z}\right) & T^{2} \left(- 1.0 a_{x} q_{y} + 1.0 a_{y} q_{x} + 1.0 a_{z} q_{w}\right) & T^{2} \left(- 1.0 a_{x} q_{z} - 1.0 a_{y} q_{w} + 1.0 a_{z} q_{x}\right)\\0 & 1 & 0 & 0 & T & 0 & T^{2} \left(1.0 a_{x} q_{z} + 1.0 a_{y} q_{w} - 1.0 a_{z} q_{x}\right) & T^{2} \left(1.0 a_{x} q_{y} - 1.0 a_{y} q_{x} - 1.0 a_{z} q_{w}\right) & T^{2} \left(1.0 a_{x} q_{x} + 1.0 a_{y} q_{y} + 1.0 a_{z} q_{z}\right) & T^{2} \left(1.0 a_{x} q_{w} - 1.0 a_{y} q_{z} + 1.0 a_{z} q_{y}\right)\\0 & 0 & 1 & 0 & 0 & T & T^{2} \left(- 1.0 a_{x} q_{y} + 1.0 a_{y} q_{x} + 1.0 a_{z} q_{w}\right) & T^{2} \left(1.0 a_{x} q_{z} + 1.0 a_{y} q_{w} - 1.0 a_{z} q_{x}\right) & T^{2} \left(- 1.0 a_{x} q_{w} + 1.0 a_{y} q_{z} - 1.0 a_{z} q_{y}\right) & T^{2} \left(1.0 a_{x} q_{x} + 1.0 a_{y} q_{y} + 1.0 a_{z} q_{z}\right)\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(- 2 a_{x} q_{z} - 2 a_{y} q_{w} + 2 a_{z} q_{x}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(2 a_{x} q_{y} - 2 a_{y} q_{x} - 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(- 2 a_{x} q_{w} + 2 a_{y} q_{z} - 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 0 & 1 & - \frac{T w_{x}}{2} & - \frac{T w_{y}}{2} & - \frac{T w_{z}}{2}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T w_{x}}{2} & 1 & \frac{T w_{z}}{2} & - \frac{T w_{y}}{2}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T w_{y}}{2} & - \frac{T w_{z}}{2} & 1 & \frac{T w_{x}}{2}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T w_{z}}{2} & \frac{T w_{y}}{2} & - \frac{T w_{x}}{2} & 1\end{matrix}\right]$



$\displaystyle \left( 10, \  10\right)$


    Measurment model jacobian (dhdx)



$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 g q_{y} & - 2 g q_{z} & 2 g q_{w} & - 2 g q_{x}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{x} & - 2 g q_{w} & - 2 g q_{z} & - 2 g q_{y}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{w} & 2 g q_{x} & 2 g q_{y} & - 2 g q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{w} & 2 m_{x} q_{x} & - 2 m_{x} q_{y} & - 2 m_{x} q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 m_{x} q_{z} & 2 m_{x} q_{y} & 2 m_{x} q_{x} & - 2 m_{x} q_{w}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{y} & 2 m_{x} q_{z} & 2 m_{x} q_{w} & 2 m_{x} q_{x}\end{matrix}\right]$



$\displaystyle \left( 9, \  10\right)$



```python
# State model jacobian (analytic solution)
dfdx_q_1stOrderApprox = eye(4) + t/2*skew4Right(wb)
dfdx_q = skew4Right(expq(0.5*t/2*wb))

dfdx = Matrix(BlockMatrix([
    [  eye(3),     t*eye(3), 0.5*t**2*dRotdq(q_toLfromB, a_B)],
    [zeros(3),       eye(3),        t*dRotdq(q_toLfromB, a_B)],
    [zeros(4,3), zeros(4,3),                           dfdx_q],

]))

# Meaurment jacobian (analytical solution)
dhdx = Matrix(BlockMatrix([
    [  eye(3), zeros(3),             zeros(3,4)],
    [zeros(3), zeros(3), dVdq(q_toLfromB, -g_L)],
    [zeros(3), zeros(3), dVdq(q_toLfromB,  B_L)],
]))

# Display
print('State model jacobian (dfdx)')
display(dfdx,dfdx.shape)
print('Measurment model jacobian (dhdx)')
display(dhdx,dhdx.shape)
```

    State model jacobian (dfdx)



$\displaystyle \left[\begin{matrix}1 & 0 & 0 & T & 0 & 0 & 0.5 T^{2} \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right) & 0.5 T^{2} \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & 0.5 T^{2} \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & 0.5 T^{2} \left(- 2 a_{x} q_{z} - 2 a_{y} q_{w} + 2 a_{z} q_{x}\right)\\0 & 1 & 0 & 0 & T & 0 & 0.5 T^{2} \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & 0.5 T^{2} \left(2 a_{x} q_{y} - 2 a_{y} q_{x} - 2 a_{z} q_{w}\right) & 0.5 T^{2} \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & 0.5 T^{2} \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right)\\0 & 0 & 1 & 0 & 0 & T & 0.5 T^{2} \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & 0.5 T^{2} \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & 0.5 T^{2} \left(- 2 a_{x} q_{w} + 2 a_{y} q_{z} - 2 a_{z} q_{y}\right) & 0.5 T^{2} \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right)\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(- 2 a_{x} q_{z} - 2 a_{y} q_{w} + 2 a_{z} q_{x}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(2 a_{x} q_{y} - 2 a_{y} q_{x} - 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right) & T \left(2 a_{x} q_{w} - 2 a_{y} q_{z} + 2 a_{z} q_{y}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(- 2 a_{x} q_{y} + 2 a_{y} q_{x} + 2 a_{z} q_{w}\right) & T \left(2 a_{x} q_{z} + 2 a_{y} q_{w} - 2 a_{z} q_{x}\right) & T \left(- 2 a_{x} q_{w} + 2 a_{y} q_{z} - 2 a_{z} q_{y}\right) & T \left(2 a_{x} q_{x} + 2 a_{y} q_{y} + 2 a_{z} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 0 & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)} & - \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)} & \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)} & \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}}\\0 & 0 & 0 & 0 & 0 & 0 & \frac{1.0 T w_{z} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \frac{1.0 T w_{y} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & - \frac{1.0 T w_{x} \sin{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}}{\sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}}} & \cos{\left(0.125 \sqrt{\left|{T w_{x}}\right|^{2} + \left|{T w_{y}}\right|^{2} + \left|{T w_{z}}\right|^{2}} \right)}\end{matrix}\right]$



$\displaystyle \left( 10, \  10\right)$


    Measurment model jacobian (dhdx)



$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 g q_{y} & - 2 g q_{z} & 2 g q_{w} & - 2 g q_{x}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{x} & - 2 g q_{w} & - 2 g q_{z} & - 2 g q_{y}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{w} & 2 g q_{x} & 2 g q_{y} & - 2 g q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{w} & 2 m_{x} q_{x} & - 2 m_{x} q_{y} & - 2 m_{x} q_{z}\\0 & 0 & 0 & 0 & 0 & 0 & - 2 m_{x} q_{z} & 2 m_{x} q_{y} & 2 m_{x} q_{x} & - 2 m_{x} q_{w}\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{y} & 2 m_{x} q_{z} & 2 m_{x} q_{w} & 2 m_{x} q_{x}\end{matrix}\right]$



$\displaystyle \left( 9, \  10\right)$


## Process Noise Covariance $Q$

Unmodeled disturbances (wind) can cause a force to act on the body. This results in an acceleration disturbance and nees to be moddeled with the process noise covariance matrix $Q$.

$$Q = \begin{bmatrix}
    \sigma_{x}^2 & 0 & 0 & \sigma_{x \dot x} & 0 & 0\\
    0 & \sigma_{y}^2 & 0 & 0 & \sigma_{y \dot y} & 0 \\
    0 & 0 & \sigma_{z}^2 & 0 & 0 & \sigma_{z \dot z}\\
    \sigma_{\dot x x} & 0 & 0 & \sigma_{\dot x}^2 & 0  & 0 \\
    0 & \sigma_{\dot y y} & 0 & 0 & \sigma_{\dot y}^2  & 0 \\
    0 & 0 & \sigma_{\dot z z} & 0 & 0 & \sigma_{\dot z}^2 \\
   \end{bmatrix} \cdot \sigma_{a}$$
   
with 𝜎𝑎 as the magnitude of the standard deviation of the acceleration, which distrubs the quadcopter. Assume no cross correlation across x,y,z dimensions.

A matrix $G$ can act as an "actor" to help with this.


```python
G = Matrix([t**2/2, t]) # one dimension
display(G*G.T)
```


$\displaystyle \left[\begin{matrix}\frac{T^{4}}{4} & \frac{T^{3}}{2}\\\frac{T^{3}}{2} & T^{2}\end{matrix}\right]$


Get the following by inspection


```python
siga = symbols('\sigma_a') # std dev
sigw = symbols('\sigma_w') # std dev

# Q - Pos and Velocity
Qpv = Matrix([
    [t**4/4,      0,      0, t**3/2,      0,      0],
    [     0, t**4/4,      0,      0, t**3/2,      0],
    [     0,      0, t**4/4,      0,      0, t**3/2],
    [t**3/2,      0,      0,   t**2,      0,      0],
    [     0, t**3/2,      0,      0,   t**2,      0],
    [     0,      0, t**3/2,      0,      0,   t**2],
]) * siga**2

# Q - quaternion
Gq = Matrix(BlockMatrix([
    [zeros(1,3)],
    [eye(3)],
]))
Gq = -t/2*skew4Left(q_toLfromB)*Gq
Qq = Gq*sigw**2*eye(3)*Gq.T

Qq2  = Matrix([
    [1-qw**2,  -qx*qw,  -qy*qw,  -qz*qw],
    [ -qw*qx, 1-qx**2,  -qy*qx,  -qz*qx],
    [ -qw*qy,  -qx*qy, 1-qy**2,  -qz*qy],
    [ -qw*qz,  -qx*qz,  -qy*qz, 1-qz**2]
]) * sigw**2*t**2/4

# Q - Full
Q = Matrix(BlockMatrix([
    [Qpv,        zeros(6,4)],
    [zeros(4,6),         Qq],
]))


print('Q - Pos and Velocity')
display(Qpv, Qpv.shape)
print('Q - Quaternion')
display(Qq, Qq.shape)
print('Q - Full')
display(Q)





```

    Q - Pos and Velocity



$\displaystyle \left[\begin{matrix}\frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0\\0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0\\0 & 0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2}\\\frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0\\0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0\\0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2}\end{matrix}\right]$



$\displaystyle \left( 6, \  6\right)$


    Q - Quaternion



$\displaystyle \left[\begin{matrix}\frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4}\\- \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4}\\- \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4}\\- \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4}\end{matrix}\right]$



$\displaystyle \left( 4, \  4\right)$


    Q - Full



$\displaystyle \left[\begin{matrix}\frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & 0 & 0 & 0 & 0\\0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & 0 & 0 & 0\\0 & 0 & \frac{T^{4} \sigma_{a}^{2}}{4} & 0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & 0 & 0\\\frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0\\0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0\\0 & 0 & \frac{T^{3} \sigma_{a}^{2}}{2} & 0 & 0 & T^{2} \sigma_{a}^{2} & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4}\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{x}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4}\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{y}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{y}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{z}^{2}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4}\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} \sigma_{w}^{2} q_{w} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{x} q_{z}}{4} & - \frac{T^{2} \sigma_{w}^{2} q_{y} q_{z}}{4} & \frac{T^{2} \sigma_{w}^{2} q_{w}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{x}^{2}}{4} + \frac{T^{2} \sigma_{w}^{2} q_{y}^{2}}{4}\end{matrix}\right]$


# Predict
## State Transition


```python
X = Matrix([
    r_L,
    v_L,
    q_toLfromB,
    gyroBias,
    accelBias
])

a_B = accelMeas-accelBias
a_L  = qRot(q_toLfromB, a_B) - g_L
a_L2 = Matrix([
    a_L[0]**2,
   a_L[1]**2,
   a_L[2]**2,
])

fx_r = r_L + v_L*t + a_L*t
fx_v = v_L + a_L*t
fx_q = (eye(4) + t/2*skew4Right(wMeas-gyroBias))*q_toLfromB
fx_gB = eye(3) * gyroBias
fx_aB = eye(3) * accelBias


fx = Matrix([
    fx_r,
    fx_v,
    fx_q,
    fx_gB,
    fx_aB
       ])


print('X')
display(X)
print('fx')
display(fx)





```

    X



$\displaystyle \left[\begin{matrix}r_{x}\\r_{y}\\r_{z}\\v_{x}\\v_{y}\\v_{z}\\q_{w}\\q_{x}\\q_{y}\\q_{z}\\g_{bx}\\g_{by}\\g_{bz}\\a_{bx}\\a_{by}\\a_{bz}\end{matrix}\right]$


    fx



$\displaystyle \left[\begin{matrix}T v_{x} + T \left(\left(- a_{bx} + a_{x}\right) \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{by} + a_{y}\right) \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{bz} + a_{z}\right) \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + r_{x}\\T v_{y} + T \left(\left(- a_{bx} + a_{x}\right) \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{by} + a_{y}\right) \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{bz} + a_{z}\right) \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + r_{y}\\T v_{z} + T \left(- g + \left(- a_{bx} + a_{x}\right) \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + \left(- a_{by} + a_{y}\right) \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + \left(- a_{bz} + a_{z}\right) \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\right) + r_{z}\\T \left(\left(- a_{bx} + a_{x}\right) \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{by} + a_{y}\right) \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{bz} + a_{z}\right) \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\right) + v_{x}\\T \left(\left(- a_{bx} + a_{x}\right) \left(2 q_{w} q_{z} + 2 q_{x} q_{y}\right) + \left(- a_{by} + a_{y}\right) \left(q_{w}^{2} - q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right) + \left(- a_{bz} + a_{z}\right) \left(- 2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\right) + v_{y}\\T \left(- g + \left(- a_{bx} + a_{x}\right) \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right) + \left(- a_{by} + a_{y}\right) \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right) + \left(- a_{bz} + a_{z}\right) \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\right) + v_{z}\\\frac{T q_{x} \left(g_{bx} - w_{x}\right)}{2} + \frac{T q_{y} \left(g_{by} - w_{y}\right)}{2} + \frac{T q_{z} \left(g_{bz} - w_{z}\right)}{2} + q_{w}\\\frac{T q_{w} \left(- g_{bx} + w_{x}\right)}{2} + \frac{T q_{y} \left(- g_{bz} + w_{z}\right)}{2} + \frac{T q_{z} \left(g_{by} - w_{y}\right)}{2} + q_{x}\\\frac{T q_{w} \left(- g_{by} + w_{y}\right)}{2} + \frac{T q_{x} \left(g_{bz} - w_{z}\right)}{2} + \frac{T q_{z} \left(- g_{bx} + w_{x}\right)}{2} + q_{y}\\\frac{T q_{w} \left(- g_{bz} + w_{z}\right)}{2} + \frac{T q_{x} \left(- g_{by} + w_{y}\right)}{2} + \frac{T q_{y} \left(g_{bx} - w_{x}\right)}{2} + q_{z}\\g_{bx}\\g_{by}\\g_{bz}\\a_{bx}\\a_{by}\\a_{bz}\end{matrix}\right]$


## State Transition Jacobian


```python
dfdx = fx.jacobian(X)

print('dfdx')
display(dfdx)
```

    dfdx



$\displaystyle \left[\begin{array}{cccccccccccccccc}1 & 0 & 0 & T & 0 & 0 & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{by} + a_{y}\right) + 2 q_{x} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & 0 & 0 & 0 & T \left(- q_{w}^{2} - q_{x}^{2} + q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- 2 q_{w} q_{y} - 2 q_{x} q_{z}\right)\\0 & 1 & 0 & 0 & T & 0 & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bz} + a_{z}\right) - 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & 0 & 0 & 0 & T \left(- 2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- q_{w}^{2} + q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{x} - 2 q_{y} q_{z}\right)\\0 & 0 & 1 & 0 & 0 & T & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bx} + a_{x}\right) - 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & 0 & 0 & 0 & T \left(2 q_{w} q_{y} - 2 q_{x} q_{z}\right) & T \left(- 2 q_{w} q_{x} - 2 q_{y} q_{z}\right) & T \left(- q_{w}^{2} + q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right)\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{by} + a_{y}\right) + 2 q_{x} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & 0 & 0 & 0 & T \left(- q_{w}^{2} - q_{x}^{2} + q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- 2 q_{w} q_{y} - 2 q_{x} q_{z}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bz} + a_{z}\right) - 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & 0 & 0 & 0 & T \left(- 2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- q_{w}^{2} + q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{x} - 2 q_{y} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{w} \left(- a_{bx} + a_{x}\right) - 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & 0 & 0 & 0 & T \left(2 q_{w} q_{y} - 2 q_{x} q_{z}\right) & T \left(- 2 q_{w} q_{x} - 2 q_{y} q_{z}\right) & T \left(- q_{w}^{2} + q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right)\\0 & 0 & 0 & 0 & 0 & 0 & 1 & \frac{T \left(g_{bx} - w_{x}\right)}{2} & \frac{T \left(g_{by} - w_{y}\right)}{2} & \frac{T \left(g_{bz} - w_{z}\right)}{2} & \frac{T q_{x}}{2} & \frac{T q_{y}}{2} & \frac{T q_{z}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T \left(- g_{bx} + w_{x}\right)}{2} & 1 & \frac{T \left(- g_{bz} + w_{z}\right)}{2} & \frac{T \left(g_{by} - w_{y}\right)}{2} & - \frac{T q_{w}}{2} & \frac{T q_{z}}{2} & - \frac{T q_{y}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T \left(- g_{by} + w_{y}\right)}{2} & \frac{T \left(g_{bz} - w_{z}\right)}{2} & 1 & \frac{T \left(- g_{bx} + w_{x}\right)}{2} & - \frac{T q_{z}}{2} & - \frac{T q_{w}}{2} & \frac{T q_{x}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T \left(- g_{bz} + w_{z}\right)}{2} & \frac{T \left(- g_{by} + w_{y}\right)}{2} & \frac{T \left(g_{bx} - w_{x}\right)}{2} & 1 & \frac{T q_{y}}{2} & - \frac{T q_{x}}{2} & - \frac{T q_{w}}{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\end{array}\right]$



```python

```


```python
qv = Matrix([q_toLfromB[1:]]).T
QF = Matrix([
    [qv, qw*eye(3)+skew3(qv)]
])
dvdq = 2*QF*Matrix([
                [zeros(1),a_B.T],
                [a_B, -skew3(a_B)]
])

C_toLfromB = quat2dcm(q_toLfromB)
box = Matrix([
    [-qv.T],
    [qw*eye(3) + skew3(qv)]
])


dfdx = Matrix([
    [zeros(3), eye(3), zeros(3,4),zeros(3),zeros(3)],
    [zeros(3), zeros(3), dvdq, zeros(3), -C_toLfromB ],
    [zeros(4,3), zeros(4,3), 1/2*skew4Right(wMeas-gyroBias), -1/2*box, zeros(4,3)],
    [zeros(3),zeros(3),zeros(3,4),zeros(3),zeros(3)],
    [zeros(3),zeros(3),zeros(3,4),zeros(3),zeros(3)]    
])
dfdx = dfdx * t + eye(16)

print('dfdx')
display(dfdx)
mprint(dfdx)
print(dfdx.shape)

```

    dfdx



$\displaystyle \left[\begin{array}{cccccccccccccccc}1 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & T & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 1 & 0 & 0 & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) - 2 q_{z} \left(a_{bz} - a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(a_{bx} - a_{x}\right)\right) & T \left(2 q_{w} \left(a_{by} - a_{y}\right) + 2 q_{x} \left(- a_{bz} + a_{z}\right) - 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & 0 & 0 & 0 & T \left(- q_{w}^{2} - q_{x}^{2} + q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- 2 q_{w} q_{y} - 2 q_{x} q_{z}\right)\\0 & 0 & 0 & 0 & 1 & 0 & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) - 2 q_{x} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(a_{bz} - a_{z}\right) - 2 q_{x} \left(- a_{by} + a_{y}\right) + 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(- 2 q_{x} \left(a_{bx} - a_{x}\right) + 2 q_{y} \left(- a_{by} + a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & T \left(2 q_{w} \left(- a_{bx} + a_{x}\right) + 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(a_{by} - a_{y}\right)\right) & 0 & 0 & 0 & T \left(- 2 q_{w} q_{z} - 2 q_{x} q_{y}\right) & T \left(- q_{w}^{2} + q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right) & T \left(2 q_{w} q_{x} - 2 q_{y} q_{z}\right)\\0 & 0 & 0 & 0 & 0 & 1 & T \left(2 q_{w} \left(- a_{bz} + a_{z}\right) + 2 q_{x} \left(- a_{by} + a_{y}\right) - 2 q_{y} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(- a_{by} + a_{y}\right) + 2 q_{x} \left(a_{bz} - a_{z}\right) + 2 q_{z} \left(- a_{bx} + a_{x}\right)\right) & T \left(2 q_{w} \left(a_{bx} - a_{x}\right) - 2 q_{y} \left(- a_{bz} + a_{z}\right) + 2 q_{z} \left(- a_{by} + a_{y}\right)\right) & T \left(2 q_{x} \left(- a_{bx} + a_{x}\right) - 2 q_{y} \left(a_{by} - a_{y}\right) + 2 q_{z} \left(- a_{bz} + a_{z}\right)\right) & 0 & 0 & 0 & T \left(2 q_{w} q_{y} - 2 q_{x} q_{z}\right) & T \left(- 2 q_{w} q_{x} - 2 q_{y} q_{z}\right) & T \left(- q_{w}^{2} + q_{x}^{2} + q_{y}^{2} - q_{z}^{2}\right)\\0 & 0 & 0 & 0 & 0 & 0 & 1 & T \left(0.5 g_{bx} - 0.5 w_{x}\right) & T \left(0.5 g_{by} - 0.5 w_{y}\right) & T \left(0.5 g_{bz} - 0.5 w_{z}\right) & 0.5 T q_{x} & 0.5 T q_{y} & 0.5 T q_{z} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & T \left(- 0.5 g_{bx} + 0.5 w_{x}\right) & 1 & T \left(- 0.5 g_{bz} + 0.5 w_{z}\right) & T \left(0.5 g_{by} - 0.5 w_{y}\right) & - 0.5 T q_{w} & 0.5 T q_{z} & - 0.5 T q_{y} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & T \left(- 0.5 g_{by} + 0.5 w_{y}\right) & T \left(0.5 g_{bz} - 0.5 w_{z}\right) & 1 & T \left(- 0.5 g_{bx} + 0.5 w_{x}\right) & - 0.5 T q_{z} & - 0.5 T q_{w} & 0.5 T q_{x} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & T \left(- 0.5 g_{bz} + 0.5 w_{z}\right) & T \left(- 0.5 g_{by} + 0.5 w_{y}\right) & T \left(0.5 g_{bx} - 0.5 w_{x}\right) & 1 & 0.5 T q_{y} & - 0.5 T q_{x} & - 0.5 T q_{w} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\end{array}\right]$


    Matrix([
    [1, 0, 0, T, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      0],
    [0, 1, 0, 0, T, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      0],
    [0, 0, 1, 0, 0, T,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      0],
    [0, 0, 0, 1, 0, 0, T*(2*q_w*(-a_bx + a_x) + 2*q_y*(-a_bz + a_z) - 2*q_z*(-a_by + a_y)), T*(2*q_x*(-a_bx + a_x) + 2*q_y*(-a_by + a_y) - 2*q_z*(a_bz - a_z)),  T*(2*q_w*(-a_bz + a_z) + 2*q_x*(-a_by + a_y) + 2*q_y*(a_bx - a_x)), T*(2*q_w*(a_by - a_y) + 2*q_x*(-a_bz + a_z) - 2*q_z*(-a_bx + a_x)),          0,          0,          0, T*(-q_w**2 - q_x**2 + q_y**2 + q_z**2),              T*(2*q_w*q_z - 2*q_x*q_y),             T*(-2*q_w*q_y - 2*q_x*q_z)],
    [0, 0, 0, 0, 1, 0, T*(2*q_w*(-a_by + a_y) - 2*q_x*(-a_bz + a_z) + 2*q_z*(-a_bx + a_x)), T*(2*q_w*(a_bz - a_z) - 2*q_x*(-a_by + a_y) + 2*q_y*(-a_bx + a_x)), T*(-2*q_x*(a_bx - a_x) + 2*q_y*(-a_by + a_y) + 2*q_z*(-a_bz + a_z)), T*(2*q_w*(-a_bx + a_x) + 2*q_y*(-a_bz + a_z) + 2*q_z*(a_by - a_y)),          0,          0,          0,             T*(-2*q_w*q_z - 2*q_x*q_y), T*(-q_w**2 + q_x**2 - q_y**2 + q_z**2),              T*(2*q_w*q_x - 2*q_y*q_z)],
    [0, 0, 0, 0, 0, 1, T*(2*q_w*(-a_bz + a_z) + 2*q_x*(-a_by + a_y) - 2*q_y*(-a_bx + a_x)), T*(2*q_w*(-a_by + a_y) + 2*q_x*(a_bz - a_z) + 2*q_z*(-a_bx + a_x)),  T*(2*q_w*(a_bx - a_x) - 2*q_y*(-a_bz + a_z) + 2*q_z*(-a_by + a_y)), T*(2*q_x*(-a_bx + a_x) - 2*q_y*(a_by - a_y) + 2*q_z*(-a_bz + a_z)),          0,          0,          0,              T*(2*q_w*q_y - 2*q_x*q_z),             T*(-2*q_w*q_x - 2*q_y*q_z), T*(-q_w**2 + q_x**2 + q_y**2 - q_z**2)],
    [0, 0, 0, 0, 0, 0,                                                                   1,                                             T*(0.5*g_bx - 0.5*w_x),                                              T*(0.5*g_by - 0.5*w_y),                                             T*(0.5*g_bz - 0.5*w_z),  0.5*T*q_x,  0.5*T*q_y,  0.5*T*q_z,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                             T*(-0.5*g_bx + 0.5*w_x),                                                                  1,                                             T*(-0.5*g_bz + 0.5*w_z),                                             T*(0.5*g_by - 0.5*w_y), -0.5*T*q_w,  0.5*T*q_z, -0.5*T*q_y,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                             T*(-0.5*g_by + 0.5*w_y),                                             T*(0.5*g_bz - 0.5*w_z),                                                                   1,                                            T*(-0.5*g_bx + 0.5*w_x), -0.5*T*q_z, -0.5*T*q_w,  0.5*T*q_x,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                             T*(-0.5*g_bz + 0.5*w_z),                                            T*(-0.5*g_by + 0.5*w_y),                                              T*(0.5*g_bx - 0.5*w_x),                                                                  1,  0.5*T*q_y, -0.5*T*q_x, -0.5*T*q_w,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          1,          0,          0,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          1,          0,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          1,                                      0,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      1,                                      0,                                      0],
    [0, 0, 0, 0, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      1,                                      0],
    [0, 0, 0, 0, 0, 0,                                                                   0,                                                                  0,                                                                   0,                                                                  0,          0,          0,          0,                                      0,                                      0,                                      1]])
    (16, 16)


## Process Noise


```python
Q_r  = (wa*t**2)**2 * eye(3)
Q_v  = (wa*t)**2 * eye(3)
Q_q  = (ww*t/2)**2 *Matrix([
    [1-qw**2,  -qx*qw,  -qy*qw,  -qz*qw],
    [ -qw*qx, 1-qx**2,  -qy*qx,  -qz*qx],
    [ -qw*qy,  -qx*qy, 1-qy**2,  -qz*qy],
    [ -qw*qz,  -qx*qz,  -qy*qz, 1-qz**2]
])
Q_gB = (wgB*t)**2 * eye(3)
Q_aB = (waB*t)**2 * eye(3)

Q = Matrix([
    [Q_r, zeros(3), zeros(3,4), zeros(3), zeros(3)],
    [zeros(3), Q_v, zeros(3,4), zeros(3), zeros(3)],
    [zeros(4,3), zeros(4,3), Q_q, zeros(4,3), zeros(4,3)],
    [zeros(3), zeros(3), zeros(3,4), Q_gB, zeros(3)],
    [zeros(3), zeros(3), zeros(3,4), zeros(3), Q_aB],
    
])

print('Q')
display(Q)
print(Q.shape)


#print('Q_r')
#display(Q_r)
#print('Q_v')
#display(Q_v)
#print('Q_q')
#display(Q_q)
#print('Q_gB')
#display(Q_gB)
#print('Q_aB')
#display(Q_aB)
```

    Q



$\displaystyle \left[\begin{array}{cccccccccccccccc}T^{4} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & T^{4} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & T^{4} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & T^{2} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & T^{2} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & T^{2} w_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & \frac{T^{2} w_{w}^{2} \left(1 - q_{w}^{2}\right)}{4} & - \frac{T^{2} q_{w} q_{x} w_{w}^{2}}{4} & - \frac{T^{2} q_{w} q_{y} w_{w}^{2}}{4} & - \frac{T^{2} q_{w} q_{z} w_{w}^{2}}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} q_{w} q_{x} w_{w}^{2}}{4} & \frac{T^{2} w_{w}^{2} \left(1 - q_{x}^{2}\right)}{4} & - \frac{T^{2} q_{x} q_{y} w_{w}^{2}}{4} & - \frac{T^{2} q_{x} q_{z} w_{w}^{2}}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} q_{w} q_{y} w_{w}^{2}}{4} & - \frac{T^{2} q_{x} q_{y} w_{w}^{2}}{4} & \frac{T^{2} w_{w}^{2} \left(1 - q_{y}^{2}\right)}{4} & - \frac{T^{2} q_{y} q_{z} w_{w}^{2}}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - \frac{T^{2} q_{w} q_{z} w_{w}^{2}}{4} & - \frac{T^{2} q_{x} q_{z} w_{w}^{2}}{4} & - \frac{T^{2} q_{y} q_{z} w_{w}^{2}}{4} & \frac{T^{2} w_{w}^{2} \left(1 - q_{z}^{2}\right)}{4} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{gB}^{2} & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{gB}^{2} & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{gB}^{2} & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{aB}^{2} & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{aB}^{2} & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & T^{2} w_{aB}^{2}\end{array}\right]$


    (16, 16)


# Update

## Measurment Function


```python
hx_accel = -qRot(quatConj(q_toLfromB), g_L)
hx_mag   =  qRot(quatConj(q_toLfromB), B_L)
hx_gps   = r_L
hx = Matrix([
    hx_accel,
    hx_mag,
    hx_gps
])

print('hx')
display(hx)
```

    hx



$\displaystyle \left[\begin{matrix}- g \left(- 2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\\- g \left(2 q_{w} q_{x} + 2 q_{y} q_{z}\right)\\- g \left(q_{w}^{2} - q_{x}^{2} - q_{y}^{2} + q_{z}^{2}\right)\\m_{x} \left(q_{w}^{2} + q_{x}^{2} - q_{y}^{2} - q_{z}^{2}\right)\\m_{x} \left(- 2 q_{w} q_{z} + 2 q_{x} q_{y}\right)\\m_{x} \left(2 q_{w} q_{y} + 2 q_{x} q_{z}\right)\\r_{x}\\r_{y}\\r_{z}\end{matrix}\right]$


## Measurment Function Jacobian


```python
dhdx = hx.jacobian(X)

print('dhdx')
display(dhdx)
mprint(dhdx)

```

    dhdx



$\displaystyle \left[\begin{array}{cccccccccccccccc}0 & 0 & 0 & 0 & 0 & 0 & 2 g q_{y} & - 2 g q_{z} & 2 g q_{w} & - 2 g q_{x} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{x} & - 2 g q_{w} & - 2 g q_{z} & - 2 g q_{y} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - 2 g q_{w} & 2 g q_{x} & 2 g q_{y} & - 2 g q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{w} & 2 m_{x} q_{x} & - 2 m_{x} q_{y} & - 2 m_{x} q_{z} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & - 2 m_{x} q_{z} & 2 m_{x} q_{y} & 2 m_{x} q_{x} & - 2 m_{x} q_{w} & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 2 m_{x} q_{y} & 2 m_{x} q_{z} & 2 m_{x} q_{w} & 2 m_{x} q_{x} & 0 & 0 & 0 & 0 & 0 & 0\\1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$


    Matrix([
    [0, 0, 0, 0, 0, 0,    2*g*q_y,  -2*g*q_z,    2*g*q_w,   -2*g*q_x, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,   -2*g*q_x,  -2*g*q_w,   -2*g*q_z,   -2*g*q_y, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,   -2*g*q_w,   2*g*q_x,    2*g*q_y,   -2*g*q_z, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,  2*m_x*q_w, 2*m_x*q_x, -2*m_x*q_y, -2*m_x*q_z, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0, -2*m_x*q_z, 2*m_x*q_y,  2*m_x*q_x, -2*m_x*q_w, 0, 0, 0, 0, 0, 0],
    [0, 0, 0, 0, 0, 0,  2*m_x*q_y, 2*m_x*q_z,  2*m_x*q_w,  2*m_x*q_x, 0, 0, 0, 0, 0, 0],
    [1, 0, 0, 0, 0, 0,          0,         0,          0,          0, 0, 0, 0, 0, 0, 0],
    [0, 1, 0, 0, 0, 0,          0,         0,          0,          0, 0, 0, 0, 0, 0, 0],
    [0, 0, 1, 0, 0, 0,          0,         0,          0,          0, 0, 0, 0, 0, 0, 0]])



```python
 
```


```python

```


```python

```


```python

```


```python

```

75e0d5d1ffabb8320309a6173e9f327a3587bee3
ipynb
Jupyter Notebook
modelDeriv/kalmanFilterMath.ipynb
lee-iv/sim-quadcopter
69d17d09a70d11f724abceea952f17339054158f
MIT
2020-07-30T00:16:29.000Z
2020-07-30T00:16:29.000Z
modelDeriv/kalmanFilterMath.ipynb
lee-iv/sim-quadcopter
69d17d09a70d11f724abceea952f17339054158f
MIT
modelDeriv/kalmanFilterMath.ipynb
lee-iv/sim-quadcopter
69d17d09a70d11f724abceea952f17339054158f
MIT
Qwen/Qwen-72B
1. YES
2. YES