Pong
June 7th 2015, GPS Smoothing Example Continued
← June 2nd 2015 GPS Smoothing Example Continued | ● | June 10th 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;
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!
← June 2nd 2015 GPS Smoothing Example Continued | ● | June 10th 2015 GPS Smoothing Example Continued →