Here’s an example of implementing a Kalman Filter in Python for a self-driving car to estimate its position and velocity based on noisy sensor data:
Code Example: Kalman Filter for Position and Velocity
import numpy as np
class KalmanFilter:
def __init__(self, dt, process_var, measurement_var):
# Time step
self.dt = dt
# State vector: [position, velocity]
self.x = np.array([[0], [0]])
# State transition matrix
self.F = np.array([[1, dt],
[0, 1]])
# Control input matrix (optional, for acceleration)
self.B = np.array([[0.5 * dt**2],
[dt]])
# Measurement matrix
self.H = np.array([[1, 0]])
# Process covariance matrix
self.Q = process_var * np.array([[dt**4 / 4, dt**3 / 2],
[dt**3 / 2, dt**2]])
# Measurement covariance matrix
self.R = np.array([[measurement_var]])
# Initial estimation covariance matrix
self.P = np.eye(2)
def predict(self, u=0):
# Predict the next state
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
def update(self, z):
# Kalman Gain
S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R
K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
# Update state estimate
y = z - np.dot(self.H, self.x)
self.x += np.dot(K, y)
# Update covariance matrix
I = np.eye(self.P.shape[0])
self.P = np.dot((I - np.dot(K, self.H)), self.P)
# Example usage
dt = 0.1 # Time step (e.g., 0.1 seconds)
kf = KalmanFilter(dt, process_var=1e-5, measurement_var=0.1)
# Simulated sensor readings (position only)
measurements = [1, 2, 3, 4, 5, 6]
for z in measurements:
kf.predict(u=0) # No acceleration in this example
kf.update(z)
print(f"Estimated Position: {kf.x[0, 0]:.2f}, Estimated Velocity: {kf.x[1, 0]:.2f}")
Explanation
State Vector: Represents the car's position and velocity.
Prediction Step: Uses the state transition matrix to predict the next state.
Update Step: Incorporates sensor measurements to correct the prediction.
Covariance Matrices: Account for uncertainties in the process and measurements.
This is a simplified example. In real-world self-driving cars, the Kalman Filter is often extended to sensor fusion (e.g., combining GPS, IMU, and LIDAR data).
Comments
Post a Comment