Currently I am designing a Kalman filter-based steering for my final paper in a driving simulator. I'm actually new to the Kalman filtering method but I've studied a couple of journals I can find, though I do have problems when designing my system. Apologies if there are mistakes in the following question since this is basically my first time into the system.

The proposed system has a purpose to predict the steering based on the target position.

From here, the state vector I'll be using is defined as

$$x = \begin{bmatrix} \alpha \\ v \\ \end{bmatrix} $$

where $\alpha$ is the steering angle and $v$ is the car's velocity. I'm also using the basic kinematics for the $F$ vector with

$$F = \begin{bmatrix} 1 & \Delta T \\ 0 & 1 \\ \end{bmatrix} $$

where $\Delta T$ will be the time step for the system. As for the measurement vector $z$, it'll comprise of $$z = \begin{bmatrix} \alpha_x \\ \alpha_y \\ v_x \\ v_y \\ \end{bmatrix} $$

which are positions and current speeds in regards to x and y axes, all in Cartesian. The positions for x and y are defined as

$$ x = \rho \times cos \theta $$ $$ y = \rho \times sin \theta $$

Where $\rho$ is the distance calculated based on the sensor readings and $\theta$ is the angle.

Given the measurements, I would also have to map the $h(x)$ vector (making this an Extended Kalman Filter) so that it'll form the eventual steering and velocity estimates using the following mapping.

$$h(\hat x) = \begin{bmatrix} arctan(\frac {2y}{x}) \\ \frac {v}{3.6} \\ \end{bmatrix} $$

With this, I have a couple of questions regarding to the difficulty I have:

1) According to the formula, the error $y$ is calculated by $z - h(\hat x)$, but given the difference of the matrices, are both the $z$ and $\hat x$ matrices being subtracted first before transformed into $h$ or not? I might perceive this the wrong way and this is one of the difficulties I have in understanding it. 2) How do I express $arctan(\frac {2y}{x})$ in matrix form such that it could be along the lines of the following normal filter $H$ matrix? $$H = \begin{bmatrix} 1 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix} $$ Given that this would eventually form the Jacobian matrices for $F_j$ and $H_j$?

Thanks in advance!


Your Answer

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

Browse other questions tagged or ask your own question.