# N-D Kalman Filter in Python + Numpy

Just implemented this Kalman Filter in Python + Numpy keeping the Wikipedia notation

It's a pretty straightforward implementation of the original algorithm, the goals were

• develop skills related to implementing a scientific paper

• keep it readable (so I have used private methods for intermediate results)

It includes a simple test case



import numpy as np

"""
Kalman Filter in plain Python + Numpy
Filter State
- x = Updated State
- P = Updated State Uncertainty
- x_pred = Predicted State
- P_pred = Predicted State Uncertainty

Models
- F = Prediction Model
- H = Observation Model (Maps from State Space to Observation Space)
- B = Evolved Forcing Term
- Q = Evolution Noise
- R = Observation Noise
"""
class KF:
def __init__(self, x, P, F, B, H, Q, R):
# Init
self.x = x
self.x_pred = x

# Initial State Uncertainty
self.P = P
self.P_pred = P

# Prediction Model
self.F = F

# Observation Model
self.H = H

# Process Noise
self.Q = Q

# Observation Noise
self.R = R

def predict(self, u):
self.x_pred = F.dot(x) + B.dot(u)
self.P_pred = F.dot(P.dot(np.transpose(F))) + Q

def __innovation(self, y):
return y - self.H.dot(self.x_pred)

def __S(self):
return self.R + self.H.dot(self.P_pred.dot(np.transpose(self.H)))

def __K(self):
return self.P_pred.dot(np.transpose(self.H).dot(np.linalg.inv(self.__S())))

def __I(self, A):
if(A.shape[0] != A.shape[1]):
raise ValueError("[Identity] Not Square")
return np.identity(A.shape[0])

def update(self, y):
self.x = self.x_pred + self.__K().dot(self.__innovation(y))
temp = self.__K().dot(self.H)
self.P = (self.__I(temp) - temp).dot(self.P_pred.dot(np.transpose(self.__I(temp) - temp))) + self.__K().dot(self.R.dot(np.transpose(self.__K())))

def to_str(self):
return "x \n" + np.array2string(self.x) + "\n P \n" + np.array2string(self.P) + "\n x_pred \n" + np.array2string(self.x_pred) + "\n P_pred \n" + np.array2string(self.P_pred)

F = np.array([[1,0], [0,1]])
P = np.array([[1,0], [0,1]])
B = np.array([[1,0], [0,1]])
H = np.array([[1,0], [0,1]])
Q = np.array([[1,0], [0,1]])
R = np.array([[1,0], [0,1]])

x = np.array([[1], [0]])
y = np.array([[3], [5]])
u = np.array([[0], [0]])

kf = KF(x, P, F, B, H, Q, R)
kf.predict(u)
kf.update(y)

print("State = " + kf.to_str())



Output

State = x
[[2.33333333]
[6.66666667]]
P
[[0.66666667 0.        ]
[0.         0.66666667]]
x_pred
[[1.]
[0.]]
P_pred
[[2. 0.]
[0. 2.]]



Validation with Filterpy as suggested by @AlexV

from filterpy.kalman import KalmanFilter
my_filter = KalmanFilter(dim_x=2, dim_z=2)
my_filter.x = x   # initial state (location and velocity)

my_filter.F = F
my_filter.H = H    # Measurement function
my_filter.P = P                 # covariance matrix
my_filter.R = R                      # state uncertainty
my_filter.Q = Q

my_filter.predict()

my_filter.update(y)

print(np.array2string(my_filter.x))



Output

[[2.33333333]
[6.66666667]]


• I assume you did this for the learning experience, but I would nevertheless like to bring filterpy to your attention. You could use it as reference to test against, but that depends on how much you trust the other implementation ;-). – AlexV Apr 25 '19 at 15:07