0
$\begingroup$

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:

model variance = 100 mm enter image description here

model variance = 1 mm enter image description here

model variance = 0.01 mm enter image description here

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:

enter image description here

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: enter image description here

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. enter image description here

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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.