I'm working on designing a Kalman Filter for more accurately predicting the position of a ultrawideband RFID tag in an indoor space. Before testing with live data, I've been playing with randomly generated data to make sure that my Kalman Filter behaves as expected, and so that I can build intuition around its behavior. So far, I've had problems with figuring out how to tune process noise to get the Kalman Estimate to converge appropriately. Here's the example I'm working with:
My Kalman Filter is pretty straightforward:
import numpy as np
class KalmanFilter():
def __init__(self, F, H, Q, R, x_0, P_0):
# Model parameters
self.F = F
self.H = H
self.Q = Q
self.R = R
# Initial state
self._x = x_0
self._P = P_0
def predict(self):
self._x = self.F @ self._x
self._P = self.F @ self._P @ self.F.transpose() + self.Q
def update(self, z):
self.S = self.H @ self._P @ self.H.transpose() + self.R
self.V = z - self.H @ self._x
self.K = self._P @ self.H.transpose() @ np.linalg.inv(self.S)
self._x = self._x + self.K @ self.V
I = np.eye(self.F.shape[1])
self._P = ((I - self.K @ self.H) @ self._P) @ (I - self.K @ self.H).transpose() + self.K @ self.R @ self.K.transpose()
#self._P = self._P - self.K @ self.S @ self.K.transpose()
def get_state(self):
return self._x, self._P
def create_3d_model_parameters(dt=1, model_variance=(0.1 ** 2, 0.1 ** 2, 0.1**2), measurement_variance=0.3**2):
# Evolution Base Matrix
# x_k = x_k-1 + dt * v_k-1
# v_k = v_k-1
f = np.array([[1, dt],
[0, 1]])
base_sigma = np.array([[1, 1],
[1, 1]])
sigma_x = model_variance[0] * base_sigma
sigma_y = model_variance[1] * base_sigma
sigma_z = model_variance[2] * base_sigma
zeros_2 = np.zeros((2, 2))
# 2d Evolution Matrix
F = np.block([[f, zeros_2, zeros_2],
[zeros_2, f, zeros_2],
[zeros_2, zeros_2, f]
])
# 2d Uncertainty Matrix
Q = np.block([[sigma_x, zeros_2, zeros_2],
[zeros_2, sigma_y, zeros_2],
[zeros_2, zeros_2, sigma_z]
])
# Measurent to State Matrix --
H = np.array([[1, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0]
])
R = measurement_variance * np.eye(3)
return F, H, Q, R
The function generate_3d_model_parameters
builds out my matrices given inputs about the system. I'm starting with a constant-velocity model, as my initial tests will be just using a Kalman Filter to determine the position of a static tag in 3d space (no movement).
I know from the sensor manufacturer that the estimated measurement error is +/- 100 mm. Given that, the only real unknown is the model (process) variance. Creating some random data and testing my filter:
import kalman
import data_generators as dg
import numpy as np
import matplotlib.pyplot as plt
if __name__ == '__main__':
(F, H, Q, R) = kalman.create_3d_model_parameters(dt=0.01, model_variance=(100**2, 100**2, 100**2), measurement_variance=100.0**2)
data_pts = 1000
# initial state
# x is the state vector [x, v_x, y, v_y, z, v_z]
x0 = np.array([0, 0, 0, 0, 0, 0])
# P is the covariance matrix
P0 = 5000 * np.eye(6)
# for generating a random truth state
#state = dg.generate_test_truth(A, Q, x0, data_pts)
state = np.zeros((data_pts, 6)) # generate state vector (0,0,0,0) as ground truth
meas = dg.generate_test_measurements(H, R, state)
kalman_filter = kalman.KalmanFilter(F, H, Q, R, x0, P0)
est_state = np.zeros((data_pts, 6))
est_cov = np.zeros((data_pts, 6, 6))
for k in range(data_pts):
kalman_filter.predict()
kalman_filter.update(meas[k, :])
(x, P) = kalman_filter.get_state()
est_state[k, :] = x
est_cov[k, ...] = P
plt.figure(figsize=(7, 5))
plt.plot(meas[:, 0], meas[:, 1], color='b', marker='o', linewidth=0, alpha=0.1, label="Observed Measurement")
plt.plot(est_state[:, 0], est_state[:, 2], color='g', marker='o', linewidth=1, label="Estimated State")
plt.plot(state[:, 0], state[:, 2], color='k', marker='o', linewidth=2, label="True State")
plt.xlabel('x [mm]')
plt.ylabel('y [mm]')
plt.legend()
plt.axis('square')
plt.tight_layout(pad=0)
plt.show()
plt.subplot(231)
plt.plot(meas[:,0], marker='o', c='b', alpha=0.1, label='Observed Measurements')
plt.plot(est_state[:,0], c='g', label='Estimated State', linewidth='2')
plt.plot(state[:,0], c='k', label='True State', linewidth='1')
plt.legend()
plt.title('Kalman Estimate')
plt.ylabel('X Position [mm]')
plt.grid(True)
plt.subplot(232)
plt.plot(meas[:,1], marker='o', c='b', alpha=0.1, label='Observed Measurements')
plt.plot(est_state[:,2], c='g', label='Estimated State', linewidth='2')
plt.plot(state[:,2], c='k', label='True State', linewidth='1')
plt.legend()
plt.title('Kalman Estimate')
plt.ylabel('Y Position [mm]')
plt.grid(True)
plt.subplot(233)
plt.plot(meas[:,2], marker='o', c='b', alpha=0.1, label='Observed Measurements')
plt.plot(est_state[:,4], c='g', label='Estimated State', linewidth='2')
plt.plot(state[:,4],c='k', label='True State', linewidth='1')
plt.legend()
plt.title('Kalman Estimate')
plt.ylabel('Z Position [mm]')
plt.grid(True)
iter = range(0, data_pts)
plt.subplot(234)
plt.plot(np.sqrt(est_cov[iter, 0, 0]),label='Error Estimate')
plt.title('Estimated Error')
plt.xlabel("Timestamp [s]")
plt.ylabel('$\sigma_{x}$ [mm]')
plt.grid(True)
plt.subplot(235)
plt.plot(np.sqrt(est_cov[iter, 2, 2]),label='Error Estimate')
plt.title('Estimated Error')
plt.xlabel("Timestamp [s]")
plt.ylabel('$\sigma_{y}$ [mm]')
plt.grid(True)
plt.subplot(236)
plt.plot(np.sqrt(est_cov[iter, 4, 4]),label='Error Estimate')
plt.title('Estimated Error')
plt.xlabel("Timestamp [s]")
plt.ylabel('$\sigma_{z}$ [mm]')
plt.grid(True)
plt.show()
I get the following results:
So far, so good -- this lines up with my intuition. As I decrease the model variance (increase certainty of my constant-velocity model), the Kalman Filter rejects noise and settles close to zero. With model variance equal to the measurement variance, the filter doesn't seem to have any reason to believe the measurements are wrong, and largely just tracks the measurements.
Switch to real data, if I use a good process variance value like 1 mm, I get the following results:
First, the results largely track the data, without any advantage of filtering. This isn't really what I was hoping for at all... Second, the error largely tracks exactly like the previous example with similar process variance, which may be related to why this happens.
Decreasing the process variance to 0.01 mm doesn't help much:
And additionally, the estimated error is decreasing to unrealistically small levels. I feel like reducing my estimate from +/- 10cm measurements to 2-5 cm or so would be realistic, but reducing the error down to single millimeters seem crazy. But the only way I can get my estimate to 'converge' and not just follow the data blindly is to reduce process error to 0.0001 mm, where my estimation error is now sub-millimeter.
So, my questions:
(1) Why does my error seem to only track from my process variance and measurement variance? How can I make the Kalman Estimate track in response to the actual data?
(2) How do I build intuition about what to set the process variance to? How do I know when the variance over-fit/under-fit for my purposes? What are the metrics I should look to? I feel like I could increase my process error for each situation, but that (1) doesn't help me in real-time situations, and (2) results in unreal levels of certainty that also seem counter-productive.