|
The Kalman Filter is an algorithm used to estimate the state of the dynamic system from the series of the noisy measurements. It is widely used in the various fields such as robotics, navigation and finance for the tasks like tracking and prediction. The Kalman Filter provides a means to the combine observed measurements with the prior knowledge about the system to the produce more accurate estimates.
What is Kalman Filter?The Kalman Filter is an optimal recursive algorithm that estimates the state of the linear dynamic system using the series of the noisy measurements. It operates in two steps: prediction and update. In the prediction step, the algorithm uses the current state estimate to the predict the next state. In the update step and it incorporates new measurements to the refine this prediction minimizing the mean of the squared error.
Step-by-Step Approach- Define Matrices: The Set up the state transition matrix, control matrix, observation matrix, process noise covariance and measurement noise covariance.
- Initial State: Initialize the state estimate and the error covariance matrix.
- Prediction Step: The Predict the next state and covariance using the state transition model.
- Update Step: Incorporate the new measurement to the update the state estimate and covariance.
- Repeat: The Iterate the prediction and update steps for the each subsequent measurement.
Implementation Code
Here is an implementation of the Kalman Filter in Python:
Python
import numpy as np
class KalmanFilter:
def __init__(self, F, B, H, Q, R, x0, P0):
self.F = F # State transition model
self.B = B
self.H = H
self.Q = Q
self.R = R
self.x = x0
self.P = P0
def predict(self, u):
# Predict the state and state covariance
self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
return self.x
def update(self, z):
# Compute the Kalman gain
S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
# Update the state estimate and covariance matrix
y = z - np.dot(self.H, self.x)
self.x = self.x + np.dot(K, y)
I = np.eye(self.P.shape[0])
self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)
return self.x
# Example usage
F = np.array([[1, 1], [0, 1]])
B = np.array([[0.5], [1]])
H = np.array([[1, 0]])
Q = np.array([[1, 0], [0, 1]])
R = np.array([[1]])
# Initial state and covariance
x0 = np.array([[0], [1]])
P0 = np.array([[1, 0], [0, 1]])
# Create Kalman Filter instance
kf = KalmanFilter(F, B, H, Q, R, x0, P0)
# Predict and update with the control input and measurement
u = np.array([[1]])
z = np.array([[1]])
# Predict step
predicted_state = kf.predict(u)
print("Predicted state:\n", predicted_state)
# Update step
updated_state = kf.update(z)
print("Updated state:\n", updated_state)
Output :
Predicted state:
[[1.5]
[2. ]]
Updated state:
[[1.125]
[1.875]]
|
