State estimation of mobile robot

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:

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

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.

• May I ask what is the reason for the downvote? – evolved Sep 16 '16 at 11:15
• 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. – Manuel Rodriguez Sep 16 '16 at 12:02
• 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. – evolved Sep 16 '16 at 12:23
• "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. – SteveO Sep 16 '16 at 16:26

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