#### An Introduction To Kalman Filter

##### Introduction

Kalman Filter, also named Linear Quadratic Estimation, are practical algorithms that have been widely used in many robotic applications, such as object tracking, navigation, etc. In this post, we focus on using Kalman filter for state estimation. The main goal of using a Kalman Filter in state estimation is to estimate the current state of a system, e.g., the current pose of a mobile robot, from a vector of sensory measurements, such IMU or GPS. Here, the sensory measurements can be exactly the system states you aim to estimate, however, due to some measurement noise, the Kalman Filter is used to filter the data and get more accurate information from the sensory inputs. Or, the measurements are just observations of the some hidden states that cannot be directly retrieved from the sensor, and hence, the Kalman Filter is used to estimate the true state of the system from noisy observations.

##### Problem Formulation

The Kalman Filter is designed to work with linear systems who dynamical models are represented via a vector of linear Ordinary Differential Equations. In addition, we represent a list of measurements using $\mathbf{y}$ and it has some relations with the system state $\mathbf{x}$.

\begin{aligned} \mathbf{\dot{x}} &= \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} & \quad \text{True Dynamics}\\ \mathbf{y} &= \mathbf{C}\mathbf{x} & \quad \text{True Observations} \\ \end{aligned}

However, the true dynamical model of the system is only avaibale to God and is intractable to us. What we can do is to approximate the system using some mathmatical models.

\begin{aligned} \mathbf{\hat{\dot{x}}} &= \mathbf{A}\mathbf{\hat{x}} + \mathbf{B}\mathbf{u} & \quad \text{Approximated Dynamics}\\ \mathbf{\hat{y}} &= \mathbf{C}\mathbf{\hat{x}} & \quad \text{Approximated Observations}\\ \end{aligned}

Objective

Our objective is to estimate the true state $\mathbf{x}$ via the approximated dynamic model and the observation.

\begin{aligned} \min \mathbf{e} &= \mathbf{x}-\mathbf{\hat{x}} \end{aligned}

We introduce a feedback control variable $K$ to regulate the estmated model and minimize the error between true state $\mathbf{x}$ and predicted state $\mathbf{\hat{x}}$.

\begin{aligned} \min \mathbf{\dot{e}} &= \mathbf{\dot{x}} - \mathbf{\hat{\dot{x}}}\\ &= \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} - (\mathbf{A}\mathbf{\hat{x}} + \mathbf{B}\mathbf{u} + K(\mathbf{y} - \mathbf{\hat{y}}) )\\ &= \mathbf{A} (\mathbf{x} - \mathbf{\hat{x}}) - KC(\mathbf{x} - \mathbf{\hat{x}}) \\ \Rightarrow \mathbf{\dot{e}} &= (A-KC)\mathbf{e}\\ \Rightarrow \mathbf{e}(t) &= e^{(A-KC)} e(0)\\ \end{aligned}

Hence, if $(A-KC)<0$, then $e \rightarrow 0$ as $t\rightarrow \infty$, and $\mathbf{\hat{x}}\rightarrow\mathbf{x}$. Therefore, the objective is to find the optimal $K$ which minimizes the difference between the true state and the approximated state.

\begin{aligned} \min_K \quad \mathbf{x}-\mathbf{\hat{x}} \end{aligned}
##### Kalman Filter

We use a Kalman Filter to find the optimal $K$, and then, the optimal state $\mathbf{x}$, by recursively update the prior estimation of the state $\mathbf{\hat{x}}$ and its error covariance matrix.

##### A Pendulum Example
"""
A Kalman Filter to estimate the state of a pendulum
(near linear system when initial angle is small)
"""
import numpy as np
#
class Kalman(object):

def __init__(self, dt):
#
self._mass = 2.0
self._gz = 9.81
self._length = 2.0
self._damping = 0.1

# System Matrix (state)
self.A = np.array([[1, dt],
[-dt*self._gz/self._length, 1]])
# System Matrix (input)
self.B = np.array([ ,  ])
# Observation Matrix (state)
self.C = np.array([ [1, 0] ])
# Observation Matrix (input)
self.D = np.array([  ])

# State error covariance matrix
self.P = np.array([ [1.0, 0.0], [0.0, 1.0] ])
# Process noise covariance matrix
self.Q = np.diag([0, 1e-4])
# Measurement noise covariance
self.R = np.array([ [1.0] ])
# Indentity matrix
self.I = np.diag([1.0, 1.0])

#
self.x = np.array([[0.0], [0.0]])
self.u = np.array([] )

def reset(self, x):
self.x = x

def predition(self,):
"""
Predicte prior state and error covariance matrix
"""
# update the state (prior)
x_prior = self.A@self.x + self.B@self.u
self.x = x_prior

# update the error covariance matrix (prior)
P_prior = self.A@self.P@self.A.T + self.Q
self.P = P_prior

def update(self, y):
"""
Update state and error covariance matrix
"""
# compute the Kalman gain
K = (self.P@self.C.T)/(self.C@self.P@self.C.T + self.R)

# Update the state (posteriror estimate)
x_post = self.x + K@(y - self.C@self.x)
self.x = x_post

# Update the error covariance matrix
p_post = (self.I - K*self.C)*self.P
self.P = p_post