2
$\begingroup$

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.

$\endgroup$
  • $\begingroup$ May I ask what is the reason for the downvote? $\endgroup$ – evolved Sep 16 '16 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$ – Manuel Rodriguez Sep 16 '16 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 Sep 16 '16 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 Sep 16 '16 at 16:26
0
$\begingroup$

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)

$\endgroup$
  • $\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 Sep 16 '16 at 11:06
  • $\begingroup$ You are welcome, the paper is a declassified version of the "Mercury context free grammar" for radar. $\endgroup$ – Manuel Rodriguez Sep 16 '16 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 Sep 16 '16 at 18:29
0
$\begingroup$

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} $$

$\endgroup$

Your Answer

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

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