This project implements a linear Kalman Filter for estimating the 3D position and velocity of a 27-gram object using noisy sensor measurements, developed as part of the RBE595 Robot Navigation course at Worcester Polytechnic Institute. The filter is evaluated across multiple data scenarios and compared against motion capture ground truth.

---

State Space

The filter tracks a 6-dimensional state vector — three spatial coordinates and their corresponding velocities:

\[\mathbf{x} = [x, y, z, \dot{x}, \dot{y}, \dot{z}]^\top\]

The system evolves according to the linear state-space model:

\(\mathbf{x}_{k+1} = A\mathbf{x}_k + B\mathbf{u}_k + \mathbf{w}_k\) \(\mathbf{z}_k = H\mathbf{x}_k + \mathbf{v}_k\)

where \(\mathbf{w}_k \sim \mathcal{N}(0, Q)\) is process noise and \(\mathbf{v}_k \sim \mathcal{N}(0, R)\) is measurement noise.

---

Filter Steps

Prediction: \(\hat{\mathbf{x}}_{k|k-1} = A\hat{\mathbf{x}}_{k-1|k-1}\) \(P_{k|k-1} = AP_{k-1|k-1}A^\top + Q\)

Update: \(K_k = P_{k|k-1}H^\top(HP_{k|k-1}H^\top + R)^{-1}\) \(\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + K_k(\mathbf{z}_k - H\hat{\mathbf{x}}_{k|k-1})\) \(P_{k|k} = (I - K_kH)P_{k|k-1}\)

---

Results

The plot below compares the Kalman-filtered trajectory against ground truth motion capture data across three scenarios: low noise, high noise, and velocity measurements.

The filter successfully suppresses noise in all scenarios, closely tracking the true trajectory even under high measurement uncertainty.

---