2
$\begingroup$

I am trying to understand some of the basics of the Kalman filter. The problem I am working on is quite complex, but I have spent some time trying to simplify the problem with an example.

So, let's say we want to track the position $p$ of a car, and the car can move left or right, at velocity $v$, in a 1-dimensional world. Noisy measurements of the car's position are being made by an external camera:

enter image description here

I am struggling to understand how to formulate a Kalman filter for this problem. What I don't understand, is whether the velocity should be included in the state or the control input, given that it is something we are actually controlling.

To show my thinking, I have created two potential implementations of the Kalman filter below. I would like to know which of these are correct, or most appropriate, and why.


1) In the first method, there is no control input. The state changes by inspecting other elements of the state.

The state at time $t$ contains position $p_t$ and velocity $v_t$:

$x_t = \begin{bmatrix}p_t\\v_t\end{bmatrix}$

The prediction stage only includes the state transition model $A$ and noise $\epsilon$; there is no control input:

$x_{t+1} = Ax_{t} + \epsilon$

The state transition model says that the car's position changes based on the velocity in the state, by calculating the velocity multiplied by the timestep $\Delta t$. And the velocity remains the same:

$A = \begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}$

We then have an observation $z_t$ of the position, which is dependent only on the position and noise $\delta$:

$z_t = Cx_{t} + \delta$

The observation model is just identity, i.e. the observation is a direction observation of the position:

$C = \begin{bmatrix}1 & 0\end{bmatrix}$

2) In the second method, we introduce a control input. The state changes by inspecting elements in both the state and the control input.

This time, the state only contains the position:

$x_t = \begin{bmatrix}p_t\end{bmatrix}$

And the state change is based on the state transition matrix, and the control input matrix.

$x_{t+1} = Ax_t + Bv_t$

This time, the state transition matrix is just identity, since we no longer include velocity and the dynamics is only due to the control input:

$A = \begin{bmatrix}1\end{bmatrix}$

And the control input matrix simply multiplies the velocity by the timestep $\Delta t$:

$B = \begin{bmatrix}\Delta t\end{bmatrix}$

And we know the velocity $v_t$ because this is the command we are controlling the car with.

And we have the same observation model as before, but this becomes a 1-by-1 matrix rather than a 1-by-2 matrix, since we have dropped the velocity component from the state vector:

$z_t = Cx_{t} + \delta$

$C = \begin{bmatrix}1\end{bmatrix}$


I don't really understand which is the most suitable method. Method 2 seems to be better than method 1, because we are directly using the velocity, rather than estimating it. And since we actually control the velocity, it makes sense to actually use it in the estimation.

But would method 1 even work? Would it be able to update its estimate of the velocity, even though the velocity is never measured or used in the dynamics model? Or can the velocity component of the state only change beyond its initial estimate if it is directly measured?

If anybody is able to help me with this, I would really appreciate it. Thank you!

$\endgroup$

1 Answer 1

1
$\begingroup$

I have split this into two main questions:

  • "whether the velocity should be included in the state or the control input"
  • "would method 1 even work?"

Question 1

The answer to question 1 is what information do you have available and what are you attempting to estimate? If you have measurement of the velocity or if estimating the velocity is not necessary, then Method 2 is probably what you are looking for.

If you require velocity estimates, from the KF, then the velocity will need to be part of the state vector.

Question 2

will it work?

So the problem here is inertial navigation, more or less. In a basic sense one would take acceleration measurements, integrate twice, and compare with the position measurements.

Method 2 is more or less inertial navigation applied with velocity measurements instead of acceleration. Therefore this method should work fine.

Method 1 on the other hand has assumed the following model for the velocity dynamics: $$ v_{k+1} = v_{k} $$ i.e. assumed that the velocity is constant. The problem here is the filtering performance is dependent on the validity of this model and the initial state estimate. I don't know your context but I imagine near constant velocity is probably not likely.

In addition it is slightly strange that there is no input at all. It kind of implies that this model is purely a zero-input response, i.e some decay from initial state for an asymptotically stable system.

If the velocity estimate is desired one would maybe expect to see something along the lines of $$ \dot{{x}} = \left[\matrix{0 &1\cr 0 &0}\right] + \left[\matrix{0 \cr 1}\right]u $$ where $u$ is the acceleration from an accelerometer. This way a model for the velocity dynamics is just simply $$ \dot{v} = a $$ This is a very simplified version of the mechanization equation for an inertial navigation system, i.e. 1 dimension and no noise.

TLDR: Where you include velocity depends on what information (read: measurements) you have available and what you want the Kalman filter to estimate.

$\endgroup$
7
  • $\begingroup$ Thanks for your answer! So, in my particular task, I don't actually need velocity estimates as an output. The only thing my system needs (to pass on to the next state of my framework) is the position. So, I am happy to not have velocity in the state vector. And I don't have a sensor directly measuring velocity; my sensor only measures position. My question is then on how to incorporate velocity: in the state vector (so that it can update the position via the state transition model), or in the control input vector (so that it can update the position via the control model). $\endgroup$ Jan 28, 2020 at 17:12
  • $\begingroup$ My confusion is that, even though I do have a control input velocity (i.e. the velocity command given to the car), I could remove the control input part of the Kalman filter, and just add velocity to the state vector. In this way, the velocity would be estimated and used to help estimate the position, even though I don't actually need the velocity as an output. But I don't understand whether this is beneficial compared to just using the velocity as a control input. My intuition is that using it as the control input is best, since it is known and doesn't need to be estimated. Is that right?! $\endgroup$ Jan 28, 2020 at 17:16
  • 1
    $\begingroup$ In your case what probably makes most sense is to use the velocity as a control signal. That way in between measurements you have an accurate model for how the control input influences the position. $\endgroup$ Jan 28, 2020 at 17:18
  • 1
    $\begingroup$ By adding the velocity as a state you remove this source of information and you augment with a velocity model that is likely not very correct, $v_{k+1}v_k$. $\endgroup$ Jan 28, 2020 at 17:20
  • 1
    $\begingroup$ Assuming that the velocity command is a relatively accurate approximate of the actual velocity then integrating to approximate the position and correcting using the Kalman filtering action should provide relatively accurate estimates, I would imagine. Of course assuming reasonably low amounts of noise on both the position and velocity information. $\endgroup$ Jan 28, 2020 at 17:22

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.