Implement Sensor Fusion with a 1D Kalman Filter in Python

1. 1
   Set Up the Simulation

   First, define the problem for our Sensor Fusion task. We'll simulate a 'true' static position and two noisy sensors measuring it. We need `numpy` for calculations. This sets the stage for fusing the sensor data to get a more accurate position.
2. 2
   Define the Kalman Update Function

   This function is the heart of our simple Sensor Fusion. It takes two Gaussian estimates (a prior belief and a new measurement, each with a mean and variance) and fuses them into a new, more confident estimate. The new variance will always be smaller than either of the inputs.
3. 3
   Initialize the State

   Before the fusion loop, we need an initial guess for our object's state. We'll start with a guess and high uncertainty (large variance). The Sensor Fusion process will quickly correct this initial guess and reduce the uncertainty.
4. 4
   Run the Sensor Fusion Loop

   Iterate through the measurements from both sensors. In each step, treat the current estimate as the 'prior' and the new sensor reading as the 'measurement'. Apply the update function to perform Sensor Fusion, refining the estimate with each new piece of data. We'll fuse Sensor 1 then Sensor 2 in each time step.

import numpy as np
import matplotlib.pyplot as plt

# 1. SIMULATION SETUP
np.random.seed(42)
TRUE_VALUE = 75.0
SENSOR1_STD_DEV = 3.0  # More accurate sensor
SENSOR2_STD_DEV = 9.0  # Less accurate sensor
NUM_MEASUREMENTS = 100

# Generate noisy measurements
measurements1 = np.random.normal(TRUE_VALUE, SENSOR1_STD_DEV, NUM_MEASUREMENTS)
measurements2 = np.random.normal(TRUE_VALUE, SENSOR2_STD_DEV, NUM_MEASUREMENTS)

# 2. KALMAN UPDATE FUNCTION
def kalman_update(prior_mean, prior_var, measurement_mean, measurement_var):
    """ Fuses two Gaussian estimates. """
    new_mean = (prior_var * measurement_mean + measurement_var * prior_mean) / (prior_var + measurement_var)
    new_var = 1 / (1 / prior_var + 1 / measurement_var)
    return new_mean, new_var

# 3. INITIALIZE STATE
estimated_pos = 60.0       # Initial guess
estimated_var = 1000.0   # High initial uncertainty
measurement_var1 = SENSOR1_STD_DEV ** 2
measurement_var2 = SENSOR2_STD_DEV ** 2
fusion_history = []

# 4. SENSOR FUSION LOOP
for i in range(NUM_MEASUREMENTS):
    # Fuse with measurement from Sensor 1
    estimated_pos, estimated_var = kalman_update(estimated_pos, estimated_var, measurements1[i], measurement_var1)
    
    # Fuse with measurement from Sensor 2
    estimated_pos, estimated_var = kalman_update(estimated_pos, estimated_var, measurements2[i], measurement_var2)
    
    fusion_history.append(estimated_pos)

print(f"True Value: {TRUE_VALUE}")
print(f"Final Fused Estimate: {estimated_pos:.2f}")
print(f"Final Estimate Uncertainty (Variance): {estimated_var:.2f}")

# 5. VISUALIZATION
plt.figure(figsize=(12, 8))
plt.plot(measurements1, 'r.', alpha=0.5, label='Sensor 1 Measurements')
plt.plot(measurements2, 'b.', alpha=0.3, label='Sensor 2 Measurements')
plt.plot(fusion_history, 'g-', lw=3, label='Fused Estimate')
plt.axhline(y=TRUE_VALUE, color='k', linestyle='--', label='True Value')
plt.title('1D Sensor Fusion with Kalman Filter')
plt.xlabel('Time Step')
plt.ylabel('Position')
plt.legend()
plt.grid(True)
plt.show()