1
$\begingroup$

I am trying to use a classical Kalman filter for getting indoor location. I am using geolocation API of HTML to get latitude, longitude, position accuracy and speed. I am also using the smartphone's accelerometer to get acceleration. The Kalman filter combines this sensor data. I tried making a simulation in python with gaussian error comparable to actual measurement. The GPS measurement has accuracy of around 20 meters.

In simulation using Kalman filter I was getting consistent accuracy of 1-2 metres for 10000 iterations. When I tried applying it in real life the location just stays at the same place. There is almost no movement when I move from the original location.

My question is how can I make the Kalman filter respond to the movement?

It should at least follow the direction even if absolute location may not be accurate initially. I looked at the histograms of the measurements. I tried using 2 methods: (a) I measured the position and aceleration after a very small interval say 1 millisecond. (b) I created a listener for when the position updates

In method b) the measurements follow a distribution very close to gaussian. In method (a) the measurements are gaussian but there is a sudden jump. Although (b) should be better but that takes a while to update so I will get fewer iterations for kalman filter hence lower accuracy. It looks like accelerometer is updating very fast but gps is not. I think need a way to deal with the missing measurements of gps. Also the two methods are tested at two different locations but origin is the same.

histogram for method (a) (in 1 dimension):

histogram for method (a) in 1 dimension

histogram for method (b) (in 1 dimension) histogram for method (b) in 1 dimension

Here is the python snippet:

  • vx, vy are velocity,
  • ax, ay are acceleration, and
  • kx,ky are the output position of the Kalman filter.
  • numpy.eye(n) is just $I_n$, the identity matrix.
class KalmanFilterLinear:
    def __init__(self, _A, _B, _H, _x, _P, _Q, _R):
        self.A = _A  # State transition matrix.
        self.B = _B  # Control matrix.
        self.H = _H  # Observation matrix.
        self.current_state_estimate = _x  # Initial state estimate.
        self.current_prob_estimate = _P  # Initial covariance estimate.
        self.Q = _Q  # Estimated error in process.
        self.R = _R  # Estimated error in measurements.

    def GetCurrentState(self):
        return self.current_state_estimate

    def Step(self, control_vector, measurement_vector, accuracymatrix):
        # ---------------------------Prediction step-----------------------------
        predicted_state_estimate = (
            self.A * self.current_state_estimate + self.B * control_vector
        )
        predicted_prob_estimate = (
            self.A * self.current_prob_estimate
        ) * numpy.transpose(self.A) + self.Q
        # --------------------------Observation step-----------------------------
        innovation = measurement_vector - self.H * predicted_state_estimate
        innovation_covariance = (
            self.H * predicted_prob_estimate * numpy.transpose(self.H) + accuracymatrix
        )
        # -----------------------------Update step-------------------------------
        kalman_gain = (
            predicted_prob_estimate
            * numpy.transpose(self.H)
            * numpy.linalg.inv(innovation_covariance)
        )
        self.current_state_estimate = (
            predicted_state_estimate + kalman_gain * innovation
        )
        # We need the size of the matrix so we can make an identity matrix.
        size = self.current_prob_estimate.shape[0]
        # eye(n) = nxn identity matrix.
        self.current_prob_estimate = (
            numpy.eye(size) - kalman_gain * self.H
        ) * predicted_prob_estimate
        # -------Creating the Kalman filter--------------------------
        timeslice = 0.0043
        state_transition = numpy.matrix(
            [[1, timeslice, 0, 0], [0, 1, 0, 0], [0, 0, 1, timeslice], [0, 0, 0, 1]]
        )
        control_matrix = numpy.matrix(
            [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
        )
        observation_matrix = numpy.eye(4)
        initial_state = numpy.matrix([[20.0], [0.0], [20.0], [0.0]])
        initial_probability = numpy.eye(4)
        process_covariance = numpy.matrix(
            [
                [timeslice ** 4 / 4, 0, timeslice ** 3 / 2, 0],
                [timeslice ** 3 / 2, 0, timeslice ** 2, 0],
                [0, timeslice ** 4 / 4, 0, timeslice ** 3 / 2],
                [0, timeslice ** 3 / 2, 0, timeslice ** 2],
            ]
        )
        measurement_covariance = numpy.eye(4) * numpy.array([20, 0.2, 20, 0.2])


kx = 0.0
ky = 0.0

kf = KalmanFilterLinear(
    state_transition,
    control_matrix,
    observation_matrix,
    initial_state,
    initial_probability,
    process_covariance,
    measurement_covariance,
)

# --------updating the kalman filter----------------------

control_vector = numpy.matrix(
    [
        [0.5 * ax * timeslice * timeslice],
        [ax * timeslice],
        [0.5 * ay * timeslice * timeslice],
        [ay * timeslice],
    ]
)
accuracymatrix = numpy.eye(4) * numpy.array([accuracy, accuracy, accuracy, accuracy])

kf.Step(control_vector, numpy.matrix([[x], [vx], [y], [vy]]), accuracymatrix)

kx = kf.GetCurrentState()[0, 0]
ky = kf.GetCurrentState()[2, 0]
$\endgroup$
10
  • $\begingroup$ @discretelizard I'm not sure I would be so quick in dismissing this as computer science. You might argue it fits in OR, Stats, SciComp, Engineering, Math, StackOverflow, but it could also be a question about the inner workings of a Kalman filter, in which case CS could be the right place. $\endgroup$
    – Pål GD
    Sep 29 '20 at 6:36
  • 2
    $\begingroup$ @PålGD The topic of the question might fit, but the question seems to be about getting a particular piece code to "work" on a smartphone, and hence is more suitable for Stack Overflow. This could turn out to be a question about Kalman filters, but it could also be a question about Python or the specific smartphone device, and given that the main information given is inside a lengthy code-block, this question did not seem suitable for Computer Science as is, and suitable for Stack Overflow. However, I can reopen the question if Dhanraj Mahurkar clarifies that this is a conceptual question about Kalman filters. $\endgroup$
    – Discrete lizard
    Sep 29 '20 at 6:48
  • $\begingroup$ fair enough ... $\endgroup$
    – Pål GD
    Sep 29 '20 at 7:04
  • $\begingroup$ I want to clarify that this is about Kalman filters and not about python .The code is just to help users understand the inputs.Would it be better if I remove the code and just include the inputs? $\endgroup$ Sep 29 '20 at 7:39
  • $\begingroup$ It's currently also posted on SO: stackoverflow.com/q/64114709/781723. Please pick only one place where you want it to appear, not both. If you want to ask here, we accept conceptual questions, but not questions about code. Debugging why your code isn't working is out of scope. I don't know why it is working better in simulation than in reality; it could be an implementation issue, it could be the quality of the data, it could be a conceptual problem, and I'm not sure how we'd tell given the information available here, or if that is on-topic here. $\endgroup$
    – D.W.
    Sep 29 '20 at 19:28

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.