Pong

## June 7th 2015, GPS Smoothing Example Continued

Again, we use the WLS solver module of libmini to solve the system of equations:

$\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)$

An example application of the module:

WLS_Point3D p1={vec3(0,0,0),0,1};
WLS_Point3D p2={vec3(1,1,1),1,1};
WLS_Point3D p3={vec3(2,2,2),2,1};

miniWLS3D wls3D;
wls3D.push_back(p1);
wls3D.push_back(p2);
wls3D.push_back(p3);
wls3D.linear_fit();

vec3 a,b;
wls3D.coefficients(a,b);

std::cout << "coefficients: a=" << a << " b=" << b << std::endl;

This code snippet fits a parametric line $\vec{ c }(t)=\vec{ a }+\vec{ b }t$ to the 3D points (0,0,0), (1,1,1) and (2,2,2) at time points 0, 1 and 2 with the weights 1, 1 and 1.

The fitted parameters $\vec{ a }$ and $\vec{ b }$ of the line are

$\vec{ a } = (0,0,0)^T$
$\vec{ b } = (1,1,1)^T$

Fits!