# Calculating Heading Error


1. To caculate Heading error we need to represent the position of the target point $G$ relative to robot frame coordinate $\{B\}$.

2. We can do this by using homogenous coordinates and transformation matrix in 2D. Here we introduce $^I\tilde{P}_G$ and $^B\tilde{P}_G$ as homogenous coordinate notation of $^I{P}_G$ and $^B{P}_G$ respectively, where.
$$
^I\tilde{P}_G =
\left(
\begin{matrix}
^Ix_G\\
^Iy_G\\
1
\end{matrix}
\right)
\ \ \text{and} \ \
^R\tilde{P}_G =
\left(
\begin{matrix}
^Bx_G\\
^By_G\\
1
\end{matrix}
\right)
$$
By using this convention we get
$$
\begin{align}
^B\tilde{P}_G &= {^BT_I}\ ^I\tilde{P}_G\\
&= {^IT_B}^{-1} \ ^I\tilde{P}_G\\
\end{align}
$$

3. We know that to represent point $B$ relative to inertial frame $\{I\}$, we need transformation matrix ${^IT_B}$ that is
$$
{^IT_B}=
\begin{pmatrix}
\cos(\theta) & -\sin(\theta) & x_B\\
\sin(\theta) & \cos(\theta) & y_B\\
0 &0 &1
\end{pmatrix}
$$
So the target point $G$ relative to robot frame coordinate $\{B\}$ is
$$
\begin{pmatrix}
^Bx_G\\
^By_G\\
1
\end{pmatrix} =
\left(
\begin{matrix}
\cos(\theta) & -\sin(\theta) & x_B\\
\sin(\theta) & \cos(\theta) & y_B\\
0 &0 &1
\end{matrix}
\right)^{-1}\
\begin{pmatrix}
^Ix_G\\
^Iy_G\\
1
\end{pmatrix}
$$

5. And we find $e_H$ as
$$
e_H = \text{atan2}(^By_G\ ,\ ^Bx_G)
$$

6. And $e_P$ as
$$
e_P = \sqrt{{^By_G}^2 + {^Bx_G}^2}
$$