# Kalman filter implementation for a driving simulation in a final project

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