Pong

## June 2nd 2015 GPS Smoothing Example Continued

Now let’s take the WLS approach from polynomial functions to the 3D case.

Each component of the position vector, that is the x-, y-, and z-coordinate of the position in ECEF coordinates, needs to be fitted to a parametric curve $\vec{ c }(t)$.

In the simplest case the curve is a straight three-dimensional line with the following parametric form:

$\vec{ c }(t) = \vec{ a } + \vec{ b }t$

We want to minimize the weighted distance of the points $\vec{ p }_i$ to the curve $\vec{ c }(t)$ so that

$\sum_i{||w_i(\vec{ c }(t_i)-\vec{ p }_i)||^2}$

becomes minimal:

$\nabla\sum_i{||w_i(\vec{ c }(t_i)-\vec{ p }_i)||^2} = 0$

This system has 6 degrees of freedom. Let’s first try to reduce the solution space by assuming that the fitted line will go through the centroid $c$ of the fitted points at median time $t$:

$\vec{ c } = \frac{\sum_i{w_i^2 \vec{ p }_i}}{\sum_i{w_i^2}}$
$t = \frac{\sum_i{w_i^2 t_i}}{\sum_i{w_i^2}}$

Substituting

$\vec{ p }'_i = \vec{ p }_i - \vec{ c }$
$t'_i = t_i - t$

yields

$\nabla\sum_i{||w_i(\vec{ p }'_i-\vec{ b } t'_i)||^2} = 0$

Taking the partial derivatives and solving for $\vec{ b }$ yields the following three linear equations:

$\sum_i{ w_i^2 (b_x {t'}_i^2 - {p'}_{i_x} {t'}_i) } = 0$
$\sum_i{ w_i^2 (b_y {t'}_i^2 - {p'}_{i_y} {t'}_i) } = 0$
$\sum_i{ w_i^2 (b_z {t'}_i^2 - {p'}_{i_z} {t'}_i) } = 0$

So that

$b_x = \frac{ \sum_i{ w_i^2 {p'}_{i_x} {t'}_i } } { \sum_i{ w_i^2 {t'}_i^2 } }$
$b_y = \frac{ \sum_i{ w_i^2 {p'}_{i_y} {t'}_i } } { \sum_i{ w_i^2 {t'}_i^2 } }$
$b_z = \frac{ \sum_i{ w_i^2 {p'}_{i_z} {t'}_i } } { \sum_i{ w_i^2 {t'}_i^2 } }$

and

$\vec{ a } = \vec{c} - \vec{b}t'$

Now for the more general case with 6 degrees of freedom:

$\nabla\sum_i{||w_i(\vec{ c }(t_i)-\vec{ p }_i)||^2} = 0$

Taking the partial derivatives yields the following six linear equations:

$\sum_i{ w_i^2 (a_x + b_x t_i - p_{i_x}) } = 0$
$\sum_i{ w_i^2 (a_y + b_y t_i - p_{i_y}) } = 0$
$\sum_i{ w_i^2 (a_z + b_z t_i - p_{i_z}) } = 0$
$\sum_i{ w_i^2 (a_x t_i + b_x t_i^2 - p_{i_x} t_i) } = 0$
$\sum_i{ w_i^2 (a_y t_i + b_y t_i^2 - p_{i_y} t_i) } = 0$
$\sum_i{ w_i^2 (a_z t_i + b_z t_i^2 - p_{i_z} t_i) } = 0$

This corresponds to the following system of linear equations:

$\left( \begin{array}{c c c c c c} \sum_i w_i^2 & 0 & 0 & \sum_i w_i^2 t_i & 0 & 0 \\ 0 & \sum_i w_i^2 & 0 & 0 & \sum_i w_i^2 t_i & 0 \\ 0 & 0 & \sum_i w_i^2 & 0 & 0 & \sum_i w_i^2 t_i \\ \sum_i w_i^2 t_i & 0 & 0 & \sum_i w_i^2 t_i^2 & 0 & 0 \\ 0 & \sum_i w_i^2 t_i & 0 & 0 & \sum_i w_i^2 t_i^2 & 0 \\ 0 & 0 & \sum_i w_i^2 t_i & 0 & 0 & \sum_i w_i^2 t_i^2 \end{array} \right) \left( \begin{array}{c} a_x \\ a_y \\ a_z \\ b_x \\ b_y \\ b_z \end{array} \right) = \left( \begin{array}{c} \sum_i w_i^2 p_{i_x} \\ \sum_i w_i^2 p_{i_y} \\ \sum_i w_i^2 p_{i_z} \\ \sum_i w_i^2 p_{i_x} t_i \\ \sum_i w_i^2 p_{i_y} t_i \\ \sum_i w_i^2 p_{i_z} t_i \end{array} \right)$

So that

$\left( \begin{array}{c} a_x \\ a_y \\ a_z \\ b_x \\ b_y \\ b_z \end{array} \right) = \left( \begin{array}{c c c c c c} \sum_i w_i^2 & 0 & 0 & \sum_i w_i^2 t_i & 0 & 0 \\ 0 & \sum_i w_i^2 & 0 & 0 & \sum_i w_i^2 t_i & 0 \\ 0 & 0 & \sum_i w_i^2 & 0 & 0 & \sum_i w_i^2 t_i \\ \sum_i w_i^2 t_i & 0 & 0 & \sum_i w_i^2 t_i^2 & 0 & 0 \\ 0 & \sum_i w_i^2 t_i & 0 & 0 & \sum_i w_i^2 t_i^2 & 0 \\ 0 & 0 & \sum_i w_i^2 t_i & 0 & 0 & \sum_i w_i^2 t_i^2 \end{array} \right)^{-1} \left( \begin{array}{c} \sum_i w_i^2 p_{i_x} \\ \sum_i w_i^2 p_{i_y} \\ \sum_i w_i^2 p_{i_z} \\ \sum_i w_i^2 p_{i_x} t_i \\ \sum_i w_i^2 p_{i_y} t_i \\ \sum_i w_i^2 p_{i_z} t_i \end{array} \right)$

For the case that the matrix is not invertible or $\vec{ b }=(0,0,0)^T$, we use the previous fit through the centroid as fallback.