import numpy as np
import matplotlib.pyplot as plt
#Parameters:
Q = 0.2     
R = 0.5     
A = 1.0
H = 1.0
x_hat = 0.0      
P = 1.0          
z = [0.7, 1.4]   
x_est = [x_hat]
P_hist = [P]


for k in range(len(z)):
    # Prediction
    x_pred = A * x_hat
    P_pred = A * P * A + Q

    # Measurement update
    y = z[k] - H * x_pred                   
    S = H * P_pred * H + R                   
    K = P_pred * H / S                       

    # Update estimates
    x_hat = x_pred + K * y
    P = (1 - K * H) * P_pred

    # Store results
    x_est.append(x_hat)
    P_hist.append(P)

    # Print step info
    print(f"Step {k+1}:")
    print(f"  K = {K:.4f}")
    print(f"  x_hat = {x_hat:.4f}")
    print(f"  P = {P:.4f}\n")

# Plots:

steps = range(len(x_est))
plt.figure(figsize=(7,4))
plt.plot(steps, x_est, 'o-', label='Kalman estimate')
plt.plot(range(1, len(z)+1), z, 'x', label='Measurements')
plt.xlabel('Step k')
plt.ylabel('x (position)')
plt.legend()
plt.grid(True)
plt.title('1D Kalman Filter Example')
plt.show()