

In [2]:

    
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm
%pylab inline --no-import-all
from sympy import Symbol, Matrix, eye, latex
from sympy.interactive import printing
printing.init_printing()









    



Populating the interactive namespace from numpy and matplotlib



In [3]:

    
def get_matrix(name='M', n=3, m=3):
    if n <= 1:
        return Matrix([[Symbol(name + '_{ ' + str(i) + '}' ) for i in range(m)]])
    elif m <= 1:
        return Matrix([[Symbol(name + '_{ ' + str(j) + '}' ) ] for j in range(n)])
    else:
        return Matrix([[Symbol(name + '_{ ' + str(i) + str(j) + '}' ) for j in range(m)] for i in range(n)])

Kalman Sympy

Dimensions



In [4]:

    
# State
n = 3

# Inputs
p = 1

# Measurement
m = 1

Matrices

State $x$



In [5]:

    
x = get_matrix('x', n, 1)
x









    Out[5]:





$$\left[\begin{matrix}x_{{ 0}}\\x_{{ 1}}\\x_{{ 2}}\end{matrix}\right]$$

Input $u$



In [6]:

    
u = get_matrix('u', p, 1)
u









    Out[6]:





$$\left[\begin{matrix}u_{{ 0}}\end{matrix}\right]$$

Measure $z$



In [7]:

    
z = get_matrix('z', m, 1)
z









    Out[7]:





$$\left[\begin{matrix}z_{{ 0}}\end{matrix}\right]$$

Model matrix $A$



In [8]:

    
A = get_matrix('A', n, n)
A









    Out[8]:





$$\left[\begin{matrix}A_{{ 00}} & A_{{ 01}} & A_{{ 02}}\\A_{{ 10}} & A_{{ 11}} & A_{{ 12}}\\A_{{ 20}} & A_{{ 21}} & A_{{ 22}}\end{matrix}\right]$$

Input matrix $B$



In [9]:

    
B = get_matrix('B', n, p)
B









    Out[9]:





$$\left[\begin{matrix}B_{{ 0}}\\B_{{ 1}}\\B_{{ 2}}\end{matrix}\right]$$

Measurement matrix $H$



In [10]:

    
H = get_matrix('H', m, n)
H









    Out[10]:





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

Process noise covariance $Q$



In [11]:

    
Q = get_matrix('Q', n, n)
Q









    Out[11]:





$$\left[\begin{matrix}Q_{{ 00}} & Q_{{ 01}} & Q_{{ 02}}\\Q_{{ 10}} & Q_{{ 11}} & Q_{{ 12}}\\Q_{{ 20}} & Q_{{ 21}} & Q_{{ 22}}\end{matrix}\right]$$

Measurement noise covariance $R$



In [12]:

    
R = get_matrix('R', m, m)
R









    Out[12]:





$$\left[\begin{matrix}R_{{ 0}}\end{matrix}\right]$$

Covariance matrix $P$



In [13]:

    
P = get_matrix('P', n, n)
P









    Out[13]:





$$\left[\begin{matrix}P_{{ 00}} & P_{{ 01}} & P_{{ 02}}\\P_{{ 10}} & P_{{ 11}} & P_{{ 12}}\\P_{{ 20}} & P_{{ 21}} & P_{{ 22}}\end{matrix}\right]$$

Modify matrices to fit application and simplify results



In [14]:

    
# Model
dt = Symbol('\Delta t')
A[0,0] = 1
A[0,1] = dt
A[0,2] = 0.5*dt**2
A[1,0] = 0
A[1,1] = 1
A[1,2] = dt
A[2,0] = 0
A[2,1] = 0
A[2,2] = 1
A









    Out[14]:





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



In [15]:

    
# Input
B[0,0] = 0.5*dt**2
B[1,0] = dt
B[2,0] = 0
B









    Out[15]:





$$\left[\begin{matrix}0.5 \Delta t^{2}\\\Delta t\\0\end{matrix}\right]$$



In [16]:

    
# Measurement
H[0,0] = 1
H[0,1] = 0
H[0,2] = 0
# H[1,0] = 1
# H[1,1] = 0
# H[1,2] = 0
# H[2,0] = 1
# H[2,1] = 0
# H[2,2] = 0
H









    Out[16]:





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



In [17]:

    
# Process covariance
s1 = Symbol('\sigma_{proc}')
v = Matrix([[0.5*dt**2], [dt], [1]])
Q = v*v.T*s1**2
Q









    Out[17]:





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



In [18]:

    
# Measurement covariance

# remove non-diagonal terms
for i in range(m):
    for j in range(m):
        if i!=j:
            R[i, j] = 0
            
R









    Out[18]:





$$\left[\begin{matrix}R_{{ 0}}\end{matrix}\right]$$

Prediction equation

$$ \hat{x}_{k+1}^{-} = \textbf{A} \cdot \hat{x}_{k} + \textbf{B} \cdot u_{k} $$



In [19]:

    
A*x + B*u









    Out[19]:





$$\left[\begin{matrix}0.5 \Delta t^{2} u_{{ 0}} + 0.5 \Delta t^{2} x_{{ 2}} + \Delta t x_{{ 1}} + x_{{ 0}}\\\Delta t u_{{ 0}} + \Delta t x_{{ 2}} + x_{{ 1}}\\x_{{ 2}}\end{matrix}\right]$$

$$ \textbf{P}_{k+1}^{-} = \textbf{A} \cdot \textbf{P}_k \cdot \textbf{A}^T + \textbf{Q}$$



In [20]:

    
A*P*A.T + Q









    Out[20]:





$$\left[\begin{matrix}P_{{ 00}} + P_{{ 10}} \Delta t + 0.5 P_{{ 20}} \Delta t^{2} + 0.25 \Delta t^{4} \sigma_{{proc}}^{2} + 0.5 \Delta t^{2} \left(P_{{ 02}} + P_{{ 12}} \Delta t + 0.5 P_{{ 22}} \Delta t^{2}\right) + \Delta t \left(P_{{ 01}} + P_{{ 11}} \Delta t + 0.5 P_{{ 21}} \Delta t^{2}\right) & P_{{ 01}} + P_{{ 11}} \Delta t + 0.5 P_{{ 21}} \Delta t^{2} + 0.5 \Delta t^{3} \sigma_{{proc}}^{2} + \Delta t \left(P_{{ 02}} + P_{{ 12}} \Delta t + 0.5 P_{{ 22}} \Delta t^{2}\right) & P_{{ 02}} + P_{{ 12}} \Delta t + 0.5 P_{{ 22}} \Delta t^{2} + 0.5 \Delta t^{2} \sigma_{{proc}}^{2}\\P_{{ 10}} + P_{{ 20}} \Delta t + 0.5 \Delta t^{3} \sigma_{{proc}}^{2} + 0.5 \Delta t^{2} \left(P_{{ 12}} + P_{{ 22}} \Delta t\right) + \Delta t \left(P_{{ 11}} + P_{{ 21}} \Delta t\right) & P_{{ 11}} + P_{{ 21}} \Delta t + \Delta t^{2} \sigma_{{proc}}^{2} + \Delta t \left(P_{{ 12}} + P_{{ 22}} \Delta t\right) & P_{{ 12}} + P_{{ 22}} \Delta t + \Delta t \sigma_{{proc}}^{2}\\P_{{ 20}} + P_{{ 21}} \Delta t + 0.5 P_{{ 22}} \Delta t^{2} + 0.5 \Delta t^{2} \sigma_{{proc}}^{2} & P_{{ 21}} + P_{{ 22}} \Delta t + \Delta t \sigma_{{proc}}^{2} & P_{{ 22}} + \sigma_{{proc}}^{2}\end{matrix}\right]$$

Correction equation

$$ \textbf{S} = \textbf{H} \cdot \textbf{P}_{k+1}^{-} \cdot \textbf{H}^T + \textbf{R} $$



In [21]:

    
S = H * P * H.T + R
#print(S.shape)
#S

$$ \textbf{K} = ( \textbf{P}_{k+1}^{-} \cdot \textbf{H}^T) \cdot \textbf{S}^{-1} $$



In [22]:

    
K = (P*H.T) * S.inv()
#print(K.shape)
#K

$$ y = z - (\textbf{H} \cdot x) $$



In [23]:

    
y = z - (H*x)
#print(y.shape)
#y

$$ \hat{x}_{k+1} = \hat{x}_{k+1}^{-} + ( \textbf{K} \cdot y) $$



In [24]:

    
x + (K*y)









    Out[24]:





$$\left[\begin{matrix}\frac{P_{{ 00}} \left(- x_{{ 0}} + z_{{ 0}}\right)}{P_{{ 00}} + R_{{ 0}}} + x_{{ 0}}\\\frac{P_{{ 10}} \left(- x_{{ 0}} + z_{{ 0}}\right)}{P_{{ 00}} + R_{{ 0}}} + x_{{ 1}}\\\frac{P_{{ 20}} \left(- x_{{ 0}} + z_{{ 0}}\right)}{P_{{ 00}} + R_{{ 0}}} + x_{{ 2}}\end{matrix}\right]$$

$$ \textbf{P}_{k+1} = (\textbf{I} - (\textbf{K} \cdot \textbf{H} ) ) \cdot \textbf{P}_{k+1}^{-} $$



In [25]:

    
I = eye(n)
(I - (K*H))*P









    Out[25]:





$$\left[\begin{matrix}P_{{ 00}} \left(- \frac{P_{{ 00}}}{P_{{ 00}} + R_{{ 0}}} + 1\right) & P_{{ 01}} \left(- \frac{P_{{ 00}}}{P_{{ 00}} + R_{{ 0}}} + 1\right) & P_{{ 02}} \left(- \frac{P_{{ 00}}}{P_{{ 00}} + R_{{ 0}}} + 1\right)\\- \frac{P_{{ 00}} P_{{ 10}}}{P_{{ 00}} + R_{{ 0}}} + P_{{ 10}} & - \frac{P_{{ 01}} P_{{ 10}}}{P_{{ 00}} + R_{{ 0}}} + P_{{ 11}} & - \frac{P_{{ 02}} P_{{ 10}}}{P_{{ 00}} + R_{{ 0}}} + P_{{ 12}}\\- \frac{P_{{ 00}} P_{{ 20}}}{P_{{ 00}} + R_{{ 0}}} + P_{{ 20}} & - \frac{P_{{ 01}} P_{{ 20}}}{P_{{ 00}} + R_{{ 0}}} + P_{{ 21}} & - \frac{P_{{ 02}} P_{{ 20}}}{P_{{ 00}} + R_{{ 0}}} + P_{{ 22}}\end{matrix}\right]$$



In [25]: