← June 1st 2015 GPS Smoothing Example Continued | ● | June 7th 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.
← June 1st 2015 GPS Smoothing Example Continued | ● | June 7th 2015 GPS Smoothing Example Continued →