This example assumes a simple 1D position estimation problem
import numpy as np
import matplotlib.pyplot as plt
# Define the true state dynamics (for simulation purposes)
dt = 1.0 # Time step
A = np.array([[1, dt], [0, 1]]) # State transition matrix
B = np.array([[0.5 * dt**2], [dt]]) # Control input matrix
H = np.array([[1, 0]]) # Measurement matrix
# True initial state
x_true = np.array([[0], [0]])
# True state noise covariance (for simulation purposes)
Q = np.array([[0.01, 0.02], [0.02, 0.1]])
# Measurement noise covariance (for simulation purposes)
R = np.array([[1]])
# Number of time steps
num_steps = 100
# Generate noisy measurements
true_states = []
measurements = []
for _ in range(num_steps):
# Simulate the true state dynamics
x_true = np.dot(A, x_true) + np.dot(B, np.array([[np.random.normal(0, Q[0, 0])]]))
# Simulate noisy measurements
z = np.dot(H, x_true) + np.array([[np.random.normal(0, R[0, 0])]])
true_states.append(x_true[0, 0])
measurements.append(z[0, 0])
# Kalman Filter Initialization
x_hat = np.array([[0], [0]]) # Initial state estimate
P = np.array([[1, 0], [0, 1]]) # Initial state estimate covariance
# Kalman Filter Estimation Loop
state_estimates = []
for z in measurements:
# Prediction Step
x_hat = np.dot(A, x_hat)
P = np.dot(np.dot(A, P), A.T) + Q
# Measurement Update Step
K = np.dot(np.dot(P, H.T), np.linalg.inv(np.dot(np.dot(H, P), H.T) + R))
x_hat = x_hat + np.dot(K, z – np.dot(H, x_hat))
P = P – np.dot(np.dot(K, H), P)
state_estimates.append(x_hat[0, 0])
# Plotting
plt.figure(figsize=(12, 6))
plt.plot(true_states, label=’True States‘, linestyle=‘–‚)
plt.plot(measurements, label=’Measurements‘, marker=’o‘, linestyle=’None‘, markersize=5)
plt.plot(state_estimates, label=’Estimated States‘)
plt.xlabel(‚Time Steps‘)
plt.ylabel(‚Position‘)
plt.legend()
plt.title(‚Kalman Filter 1D Position Estimation‘)
plt.grid(True)
plt.show()
This entry was posted on Freitag, September 8th, 2023 at 11:35 and is filed under Administration. You can follow any responses to this entry through the RSS 2.0 feed.
You can leave a response, or trackback from your own site.