parent
c22cab503b
commit
e23e2e24b2
Binary file not shown.
@ -0,0 +1,216 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Kalman Filter for Angular Position/Velocity/Acceleration
|
||||||
|
=========================================================
|
||||||
|
1D Kalman filter for tracking door angle from noisy encoder readings.
|
||||||
|
|
||||||
|
State vector: [angle, velocity, acceleration]
|
||||||
|
Measurement: angle (from 14-bit I2C encoder)
|
||||||
|
|
||||||
|
Author: Kynsight
|
||||||
|
Version: 1.0.0
|
||||||
|
"""
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from typing import Tuple, Optional
|
||||||
|
|
||||||
|
|
||||||
|
class AngleKalmanFilter:
|
||||||
|
"""
|
||||||
|
Kalman filter for angular position tracking.
|
||||||
|
|
||||||
|
Tracks angle (deg), angular velocity (deg/s), and angular acceleration (deg/s²).
|
||||||
|
Designed for 14-bit encoder with 0.022° quantization noise.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
process_noise: float = 0.1,
|
||||||
|
measurement_noise: float = 0.022,
|
||||||
|
initial_angle: float = 0.0
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Initialize Kalman filter.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
process_noise: Process noise covariance (system uncertainty)
|
||||||
|
measurement_noise: Measurement noise (encoder quantization = 0.022°)
|
||||||
|
initial_angle: Initial angle estimate (degrees)
|
||||||
|
"""
|
||||||
|
# State vector: [angle, velocity, acceleration]
|
||||||
|
self.x = np.array([initial_angle, 0.0, 0.0])
|
||||||
|
|
||||||
|
# State covariance matrix (uncertainty in estimates)
|
||||||
|
self.P = np.eye(3) * 1.0
|
||||||
|
|
||||||
|
# Process noise covariance
|
||||||
|
self.Q = np.array([
|
||||||
|
[process_noise, 0, 0],
|
||||||
|
[0, process_noise * 10, 0], # Velocity more uncertain
|
||||||
|
[0, 0, process_noise * 100] # Acceleration even more uncertain
|
||||||
|
])
|
||||||
|
|
||||||
|
# Measurement noise covariance (14-bit encoder = 360/16384 = 0.022°)
|
||||||
|
self.R = np.array([[measurement_noise ** 2]])
|
||||||
|
|
||||||
|
# Measurement matrix (we only measure angle)
|
||||||
|
self.H = np.array([[1.0, 0.0, 0.0]])
|
||||||
|
|
||||||
|
# Previous timestamp for dt calculation
|
||||||
|
self.prev_time_ns: Optional[int] = None
|
||||||
|
|
||||||
|
# Filter initialized flag
|
||||||
|
self.initialized = False
|
||||||
|
|
||||||
|
def predict(self, dt: float):
|
||||||
|
"""
|
||||||
|
Predict step: propagate state forward in time.
|
||||||
|
|
||||||
|
State transition model:
|
||||||
|
angle(k+1) = angle(k) + velocity(k)*dt + 0.5*accel(k)*dt²
|
||||||
|
velocity(k+1) = velocity(k) + accel(k)*dt
|
||||||
|
accel(k+1) = accel(k) (constant acceleration model)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
dt: Time step in seconds
|
||||||
|
"""
|
||||||
|
# State transition matrix
|
||||||
|
F = np.array([
|
||||||
|
[1.0, dt, 0.5 * dt**2],
|
||||||
|
[0.0, 1.0, dt],
|
||||||
|
[0.0, 0.0, 1.0]
|
||||||
|
])
|
||||||
|
|
||||||
|
# Predict state
|
||||||
|
self.x = F @ self.x
|
||||||
|
|
||||||
|
# Predict covariance
|
||||||
|
self.P = F @ self.P @ F.T + self.Q
|
||||||
|
|
||||||
|
def update(self, measurement: float):
|
||||||
|
"""
|
||||||
|
Update step: correct prediction with measurement.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
measurement: Measured angle in degrees
|
||||||
|
"""
|
||||||
|
# Measurement residual
|
||||||
|
z = np.array([[measurement]])
|
||||||
|
y = z - self.H @ self.x.reshape(-1, 1)
|
||||||
|
|
||||||
|
# Residual covariance
|
||||||
|
S = self.H @ self.P @ self.H.T + self.R
|
||||||
|
|
||||||
|
# Kalman gain
|
||||||
|
K = self.P @ self.H.T @ np.linalg.inv(S)
|
||||||
|
|
||||||
|
# Update state
|
||||||
|
self.x = self.x + (K @ y).flatten()
|
||||||
|
|
||||||
|
# Update covariance
|
||||||
|
I = np.eye(3)
|
||||||
|
self.P = (I - K @ self.H) @ self.P
|
||||||
|
|
||||||
|
def process(
|
||||||
|
self,
|
||||||
|
angle_measurement: float,
|
||||||
|
timestamp_ns: int
|
||||||
|
) -> Tuple[float, float, float]:
|
||||||
|
"""
|
||||||
|
Process one measurement (predict + update).
|
||||||
|
|
||||||
|
Args:
|
||||||
|
angle_measurement: Raw angle measurement (degrees)
|
||||||
|
timestamp_ns: Timestamp in nanoseconds
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
(filtered_angle, filtered_velocity, filtered_acceleration)
|
||||||
|
"""
|
||||||
|
if not self.initialized:
|
||||||
|
# First measurement - initialize state
|
||||||
|
self.x[0] = angle_measurement
|
||||||
|
self.x[1] = 0.0
|
||||||
|
self.x[2] = 0.0
|
||||||
|
self.prev_time_ns = timestamp_ns
|
||||||
|
self.initialized = True
|
||||||
|
return (angle_measurement, 0.0, 0.0)
|
||||||
|
|
||||||
|
# Calculate time step
|
||||||
|
dt = (timestamp_ns - self.prev_time_ns) / 1_000_000_000.0
|
||||||
|
self.prev_time_ns = timestamp_ns
|
||||||
|
|
||||||
|
if dt <= 0:
|
||||||
|
# Duplicate timestamp - return previous estimate
|
||||||
|
return (self.x[0], self.x[1], self.x[2])
|
||||||
|
|
||||||
|
# Predict
|
||||||
|
self.predict(dt)
|
||||||
|
|
||||||
|
# Update with measurement
|
||||||
|
self.update(angle_measurement)
|
||||||
|
|
||||||
|
return (self.x[0], self.x[1], self.x[2])
|
||||||
|
|
||||||
|
def reset(self, angle: float = 0.0):
|
||||||
|
"""
|
||||||
|
Reset filter to initial state.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
angle: Initial angle estimate
|
||||||
|
"""
|
||||||
|
self.x = np.array([angle, 0.0, 0.0])
|
||||||
|
self.P = np.eye(3) * 1.0
|
||||||
|
self.prev_time_ns = None
|
||||||
|
self.initialized = False
|
||||||
|
|
||||||
|
def get_state(self) -> Tuple[float, float, float]:
|
||||||
|
"""
|
||||||
|
Get current state estimate.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
(angle, velocity, acceleration)
|
||||||
|
"""
|
||||||
|
return (self.x[0], self.x[1], self.x[2])
|
||||||
|
|
||||||
|
|
||||||
|
# =============================================================================
|
||||||
|
# Demo
|
||||||
|
# =============================================================================
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
print("Kalman Filter Demo")
|
||||||
|
print("=" * 60)
|
||||||
|
|
||||||
|
# Create filter
|
||||||
|
kf = AngleKalmanFilter(
|
||||||
|
process_noise=0.1,
|
||||||
|
measurement_noise=0.022, # 14-bit encoder quantization
|
||||||
|
initial_angle=-84.0
|
||||||
|
)
|
||||||
|
|
||||||
|
# Simulate noisy measurements
|
||||||
|
import random
|
||||||
|
|
||||||
|
print("\nSimulating noisy angle measurements:")
|
||||||
|
print(f"{'Time (ms)':<12} {'Raw Angle':<12} {'Filtered':<12} {'Velocity':<12} {'Accel':<12}")
|
||||||
|
print("-" * 60)
|
||||||
|
|
||||||
|
true_angle = -84.0
|
||||||
|
time_ns = 0
|
||||||
|
|
||||||
|
for i in range(20):
|
||||||
|
time_ns += 35_000_000 # ~35ms intervals (typical packet rate)
|
||||||
|
|
||||||
|
# Add quantization noise (0.022° steps)
|
||||||
|
noise = random.choice([-0.022, 0, 0.022])
|
||||||
|
measurement = true_angle + noise
|
||||||
|
|
||||||
|
# Process measurement
|
||||||
|
angle, vel, accel = kf.process(measurement, time_ns)
|
||||||
|
|
||||||
|
print(f"{time_ns/1e6:<12.1f} {measurement:<12.4f} {angle:<12.4f} {vel:<12.4f} {accel:<12.4f}")
|
||||||
|
|
||||||
|
# Slowly increase angle (simulate door opening)
|
||||||
|
true_angle += 0.02
|
||||||
|
|
||||||
|
print("\n✓ Filter smooths quantization noise while tracking motion")
|
||||||
Loading…
Reference in new issue