For a mobile robot - four wheels, front wheel steering - I use the following (bicycle) prediction model to estimate its state based on accurate radar measurements only. No odometry or any other input information $u_k$ is available from the mobile robot itself.

$$ \begin{bmatrix} x_{k+1} \\ y_{k+1} \\ \theta_{k+1} \\ v_{k+1} \\ a_{k+1} \\ \kappa_{k+1} \\ \end{bmatrix} = f_k(\vec{x}_k,u_k,\vec{\omega}_k,\Delta t) = \begin{bmatrix} x_k + v_k \Delta t \cos \theta_k \\ y_k + v_k \Delta t \sin \theta_k \\ \theta_k + v_k \kappa_k \Delta t \\ v_k + a_k \Delta t \\ a_k \\ \kappa_k + \frac{a_{y,k}}{v_{x,k}^2} \end{bmatrix} + \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_{\theta} \\ \omega_v \\ \omega_a \\ \omega_{\kappa} \end{bmatrix} $$

where $x$ and $y$ are the position, $\theta$ is the heading and $v$, $a$ are the velocity and acceleration respectively. Vector $\vec{\omega}$ is zero mean white gaussian noise and $\Delta t$ is sampling time. These mentioned state variables $\begin{bmatrix} x & y & \theta & v & a \end{bmatrix}$ are all measured although $\begin{bmatrix} \theta & v & a \end{bmatrix}$ have high variance. The only state that is not measured is curvature $\kappa$. Therfore it is computed using the measured states $\begin{bmatrix} a_{y,k} & v_{x,k}^2\end{bmatrix}$ which are the lateral acceleration and the longitudinal velocity.

My Question:

Is there a better way on predicting heading $\theta$, velocity $v$, acceleration $a$, and curvature $\kappa$?

  • Is it enough for $a_{k+1}$ to just assume gaussian noise $\omega_a$ and use the previous best estimate $a_k$ or is there an alternative?

  • For curvature $\kappa$ I also thought of using yaw rate $\dot{\theta}$ as $\kappa = \frac{\dot{\theta}}{v_x}$ but then I would have to estimate the yaw rate too.

To make my nonlinear filter model complete here is the measurement model:

$$ \begin{equation} \label{eq:bicycle-model-leader-vehicle-h} y_k = h_k(x_k,k) + v_k = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ \end{bmatrix} \begin{bmatrix} x_k \\ y_k \\ \theta_k \\ v_k \\ a_k \\ \kappa_k \\ \end{bmatrix} + \begin{bmatrix} v_x \\ v_y \\ v_{\theta} \\ v_v \\ v_a \\ \end{bmatrix} \end{equation} $$

More Info on the available data:

The measured state vector is already obtained/estimated using a kalman filter. What I want to achive is a smooth trajectory with the estimate $\kappa$. For this it is a requirement to use another Kalman filter or a moving horizon estimation approach.

  • $\begingroup$ May I ask what is the reason for the downvote? $\endgroup$
    – evolved
    Commented Sep 16, 2016 at 11:15
  • $\begingroup$ Sure, i downvoted the question, because matrix oriented prediction model is based on analog pid-controller which are used in the 1930's before the first computers were invented. It is not state-of-the-art. $\endgroup$ Commented Sep 16, 2016 at 12:02
  • 1
    $\begingroup$ ok this may be the case - that it is not state of the art - but I think this is not a reason to downvote. As this "matrix oriented prediction" is still a possible approach which get's used frequently and I am required to use it. $\endgroup$
    – evolved
    Commented Sep 16, 2016 at 12:23
  • 4
    $\begingroup$ "Not state of the art" is wrong. The IEEE has 20 conference publications, 10 journal articles, and 8 other articles THIS YEAR that discuss both Kalman Filter and State Space control. This is a proven technique that continues to find broad applicability for very challenging control problems. $\endgroup$
    – SteveO
    Commented Sep 16, 2016 at 16:26
  • $\begingroup$ @ManuelRodriguez is right that alternative solutions exist that may be more advanced (though his proposed solution is at best arguably the next candidate), but ultimately incorrect: the Kalman filter is absolutely still (begrudgingly) the state of the art in state estimation. $\endgroup$ Commented Jan 20, 2022 at 16:34

2 Answers 2


My solution is to use the following model with disturbance only at acceleration and curvature.

$$ \begin{bmatrix} x_{k+1} \\ y_{k+1} \\ \theta_{k+1} \\ v_{k+1} \\ a_{k+1} \\ \kappa_{k+1} \\ \end{bmatrix} = f_k(\vec{x}_k,u_k,\vec{\omega}_k,\Delta t) = \begin{bmatrix} x_k + v_k \Delta t \cos \theta_k \\ y_k + v_k \Delta t \sin \theta_k \\ \theta_k + v_k \kappa_k \Delta t \\ v_k + a_k \Delta t \\ a_k + \omega_a \\ \kappa_k + \omega_{\kappa} \end{bmatrix} $$


Kalman filters and other state-estimation techniques are based on analog computing. In most cases differential equations are used. A better solution is based on grammar techniques aka turing-machines. A paper which is using context free grammars for state-estimation is given here. The idea in short: the measurements are interpreted as a stream of tokens like a computerlanguage and a parser interprets the sentences. To generate such parser motion primitive are used (simple Finite-States-Machines which describes a possible movement of the robot)

  • $\begingroup$ Thanks for your help, I'll try to get some ideas out of the paper. Unfortunately the requeirements are that I use Kalman filters or moving horizon estimation. $\endgroup$
    – evolved
    Commented Sep 16, 2016 at 11:06
  • $\begingroup$ You are welcome, the paper is a declassified version of the "Mercury context free grammar" for radar. $\endgroup$ Commented Sep 16, 2016 at 11:17
  • $\begingroup$ From the cited paper: The more traditional approach such as hidden Markov and state space models are suitable for target modeling [22], [23], but not radar modeling. I think the authors of that paper are solving a much more stochastic problem than you present. If you do not have any other odometry from the mobile robot, then your approach seems reasonable. You might be able to implement a feedforward model to account for friction, inclines, and other nonlinearities, but without local feedback even that would be a challenge. $\endgroup$
    – SteveO
    Commented Sep 16, 2016 at 18:29
  • 1
    $\begingroup$ This answer is not directed at the question, and is actually somewhat misleading. $\endgroup$ Commented Jan 20, 2022 at 16:36

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.