From e0d20f26ef62e0a677164de5a64d4e0a7a353d8c Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Fri, 18 Sep 2020 17:00:19 +0200
Subject: [PATCH] #570 Added documentation for LH2 to LH1 angle conversion

---
 .../lighthouse/angle_conversion.md            | 52 +++++++++++++++++++
 docs/functional-areas/lighthouse/index.md     |  1 +
 2 files changed, 53 insertions(+)
 create mode 100644 docs/functional-areas/lighthouse/angle_conversion.md

diff --git a/docs/functional-areas/lighthouse/angle_conversion.md b/docs/functional-areas/lighthouse/angle_conversion.md
new file mode 100644
index 0000000000..afeb80cebf
--- /dev/null
+++ b/docs/functional-areas/lighthouse/angle_conversion.md
@@ -0,0 +1,52 @@
+---
+title: Lighthouse angle conversion
+page_id: lh_angle_conversion
+---
+
+One way to get started with lighthouse 2 is to create a conversion from lighthouse 2 angles to lighthouse 1 angles, and
+use the functionality that has already been implemented for lighthouse 1. Even though it is an easy way to get started,
+the main drawback is that we need both sweeps for the conversion.
+
+The strategy when creating an equation to convert from lighthouse 2 sweep angles to lighthouse 1, is to find the intersection
+line between the two light planes. From the intersection line we can easily calculate the lighthouse 1 sweep angles.
+
+The following calculations are all in the base station reference frame.
+
+Assume we know the normals for the two light planes, $$\vec{n_1}$$ and $$\vec{n_2}$$. The cross product of the normals $$\vec{v} = \vec{n_1} \times \vec{n_2}$$
+gives us the direction of the intersection line. We also know it is passing through the origin as we are using the base station
+reference frame.
+
+## Normals of the light planes
+
+Start by figuring out the normals when $$\alpha=0$$, and then rotate them around the Z-axis using a rotation matrix.
+
+The rotation matrix is
+
+$$R_{z} = \left[\begin{array}{ccc}
+cos{\alpha} & -sin{\alpha} & 0 \\
+sin{\alpha} & cos{\alpha} & 0 \\
+0 & 0 & 1
+\end{array}\right]$$
+
+and the resulting normals
+
+$$\vec{n1}=R_z \cdot \begin{bmatrix}0 & -cos{(t)} & sin{(t)}\end{bmatrix} = \begin{bmatrix}cos{(t)}sin{(\alpha^{lh2}_1)} & -cos{(t)}cos{(\alpha^{lh2}_1)} & sin{(t)}\end{bmatrix}$$
+
+$$\vec{n2}=R_z \cdot \begin{bmatrix}0 & -cos{(t)} & -sin{(t)}\end{bmatrix} = \begin{bmatrix}cos{(t)}sin{(\alpha^{lh2}_2)} & -cos{(t)}cos{(\alpha^{lh2}_2)} & -sin{(t)}\end{bmatrix}$$
+
+where $$t=\pi/6$$ is the tilt angle of the light planes.
+
+
+## The intersection vector
+
+$$\vec{v} = \vec{n_1} \times \vec{n_2} = \begin{bmatrix}-\sin{(t)}\cos{(t)}(\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)}) & -\sin{(t)}\cos{(t)}(\sin{(\alpha^{lh2}_1)} + \sin{(\alpha^{lh2}_2)}) & \cos^2{(t)}(\sin{(\alpha^{lh2}_1)}\cos{(\alpha^{lh2}_2)}-\cos{(\alpha^{lh2}_1)}\sin{(\alpha^{lh2}_2)})\end{bmatrix}$$
+
+## Lighthouse 1 angles
+
+Finally we can calculate the lighthouse 1 angles
+
+$$\alpha^{lh1}_1 = \tan^{-1}(\frac{v_2}{v_1}) = \tan^{-1}(\frac{-\sin{(t)}\cos{(t)}(\sin{(\alpha^{lh2}_1)} + \sin{(\alpha^{lh2}_2)})}{-\sin{(t)}\cos{(t)}(\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)})}) = \frac{\alpha^{lh2}_1 + \alpha^{lh2}_2}{2}$$
+
+$$\alpha^{lh1}_2 = \tan^{-1}(\frac{v_3}{v_1}) =
+\tan^{-1}(\frac{\cos^2{(t)}(\sin{(\alpha^{lh2}_1)}\cos{(\alpha^{lh2}_2)}-\cos{(\alpha^{lh2}_1)}\sin{(\alpha^{lh2}_2)})}{-\sin{(t)}\cos{(t)}(\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)})}) =
+\tan^{-1}(\frac{\sin{(\alpha^{lh2}_2 - \alpha^{lh2}_1)}}{\tan{(t)} (\cos{(\alpha^{lh2}_1)} + \cos{(\alpha^{lh2}_2)})})$$
diff --git a/docs/functional-areas/lighthouse/index.md b/docs/functional-areas/lighthouse/index.md
index cb355ee306..4c34b19d99 100644
--- a/docs/functional-areas/lighthouse/index.md
+++ b/docs/functional-areas/lighthouse/index.md
@@ -19,4 +19,5 @@ The basics:
 
 Development related pages
  * [Terminology and definitions](/docs/functional-areas/lighthouse/terminology_definitions.md)
+ * [Conversion from LH2 angles to LH1](/docs/functional-areas/lighthouse/angle_conversion.md)
  * [Kalman estimator measurement model](/docs/functional-areas/lighthouse/kalman_measurement_model.md)