Vehicle State Jacobian

Solving the Jacobian for every variable in the system.

Because GPS rotates in the clockwise direction, and traditional math assumes a $\theta$ increase in the counter-clockwise direction, we can calculate the mathematical $\theta$ by doing: $90°-{\theta }_{\text{GPS}}°$. This is the angle used in all the following calculations.

Recall the system matrices:

System Matrices

The following is the system’s state matrix, $\hat{x}$:

$$\hat{x}=\left[\begin{array}{c}x\\ \dot{x}\\ y\\ \dot{y}\\ \theta \end{array}\right]$$

With the given inputs, we can define our control matrix, $u$:

$$u=\left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \dot{\theta }\end{array}\right]$$

Because we have a theta, we will also be using a rotation matrix to globalize the vehicles positioning.

And all the perfect estimate equations are:

$$\text{system perfect estimate}=\begin{array}{r}\{\begin{array}{l}{x}_{\text{new}}=x_{\text{old}}+v_{x_{\text{old}}}\times \Delta t+\frac{1}{2}\Delta t^2(a_{x_{\text{old}}}\cos (\theta )-a_{y_{\text{old}}}\sin (\theta ))\\ y_{\text{new}}=y_{\text{old}}+v_{y_{\text{old}}}\times \Delta t+\frac{1}{2}\Delta t^2(a_{x_{\text{old}}}\sin (\theta )+a_{y_{\text{old}}}\cos (\theta ))\\ {\dot{x}}_{\text{new}}={\dot{x}}_{\text{old}}+\Delta t(a_{x_{\text{old}}}\cos (\theta )-a_{y_{\text{old}}}\sin (\theta ))\\ {\dot{y}}_{\text{new}}={\dot{y}}_{\text{old}}+\Delta t(a_{x_{\text{old}}}\sin (\theta )+a_{y_{\text{old}}}\cos (\theta ))\\ {\theta }_{\text{new}}=\omega \times \Delta t+{\theta }_{\text{old}}\end{array}\end{array}$$ Link to original

As a result, we can calculate the partial derivatives of each estimate as such:

Equation ($\text{eq.}$)	$\frac{\partial \text{eq.}}{\partial x_{\text{old}}}$	$\frac{\partial \text{eq.}}{\partial y_{\text{old}}}$	$\frac{\partial \text{eq.}}{\partial {\dot{x}}_{\text{old}}}$	$\frac{\partial \text{eq.}}{\partial {\dot{y}}_{\text{old}}}$	$\frac{\partial \text{eq.}}{\partial {\theta }_{\text{old}}}$
$x_{\text{new}}=x_{\text{old}}+v_{x_{\text{old}}}\times \Delta t+\frac{1}{2}\Delta t^2(a_{x_{\text{old}}}\cos (\theta )-a_{y_{\text{old}}}\sin (\theta ))$	1	0	$\Delta t$	0	$\frac{1}{2}\Delta t^2(-a_{x_{\text{old}}}\sin (\theta )-a_{y_{\text{old}}}\cos (\theta ))$
$y_{\text{new}}=y_{\text{old}}+v_{y_{\text{old}}}\times \Delta t+\frac{1}{2}\Delta t^2(a_{x_{\text{old}}}\sin (\theta )+a_{y_{\text{old}}}\cos (\theta ))$	0	1	0	$\Delta t$	$\frac{1}{2}\Delta t^2(a_{x_{\text{old}}}\cos (\theta )-a_{y_{\text{old}}}\sin (\theta ))$
${\dot{x}}_{\text{new}}={\dot{x}}_{\text{old}}+\Delta t(a_{x_{\text{old}}}\cos (\theta )-a_{y_{\text{old}}}\sin (\theta ))$	0	0	1	0	$\Delta t(-a_{x_{\text{old}}}\sin (\theta )-a_{y_{\text{old}}}\cos (\theta ))$
${\dot{y}}_{\text{new}}={\dot{y}}_{\text{old}}+\Delta t(a_{x_{\text{old}}}\sin (\theta )+a_{y_{\text{old}}}\cos (\theta ))$	0	0	0	1	$\Delta t(a_{x_{\text{old}}}\cos (\theta )-a_{y_{\text{old}}}\sin (\theta ))$
${\theta }_{\text{new}}=\omega \times \Delta t+{\theta }_{\text{old}}$	0	0	0	0	1