I am trying to set up a Kalman-Filter to filter position-measurements of a self-driving car. To do so, I consider a state-vector with 5 elements and am now trying to set up the Transition Matrix.

As you can see in the following picture, I do not have a clear understanding of the car dynamics. How does my steering for example contribute to my x-position? Since steering is the rate of change of the vehicle heading, it must somehow be linked to acceleration. Can anyone help me to figure out A ?

I would be so grateful!



1 Answer 1


When localizing a robot we usually use the world coordinate frame. Since it's static it is easier to represent the position of the robot. As an example you may use the North and East directions as the x and y axis of the world coordinate frame. But if you are travelling on the robot you can notice that the heading direction always changes. Let's take 2 cases to understand this. 1. Robot is travelling to the absolute north with a speed of 1m per second. 2. Robot is travelling to the absolute north-west with a speed of 1m per second.

In both cases robot travels towards its' heading direction but in world coordinate frame the heading is different. enter image description here

We can calculate the A matrix by assuming that there is no control inputs to the system.

  • $\begingroup$ That's great, thx so much! Just two questions about your answer: The matrix you have given assumes that the steering is constant. Does this mean we are constantly steering in the same direction? ..I have a feeling this cannot be and I probably just misunderstand the last equation. Second: What if we consider control input? In a car, we can control acceleration. But this would have to be considered in a separate matrix, right? So that x_t = A x_{t-1} + B u_{t-1}, where u is the input vector $\endgroup$
    – user503842
    Commented Jan 2, 2019 at 17:41
  • $\begingroup$ Here we only consider about the state transition matrix by ignoring the control inputs. For a real world system the steering can be directly applied to the theta_dot and discard the theta_dot_{t-1}. Therefore in this case the best method is to include the control input matrix as well. $\endgroup$ Commented Jan 3, 2019 at 18:22
  • $\begingroup$ allright, thx again! Just to be clear: The control input matrix is what I am referring to as matrix B in my previous comment, am I right? $\endgroup$
    – user503842
    Commented Jan 3, 2019 at 21:46
  • $\begingroup$ Yes. The deterministic control inputs can be passed in to the state matrix using B. $\endgroup$ Commented Jan 4, 2019 at 5:46

Your Answer

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

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