aboutsummaryrefslogtreecommitdiff
path: root/apps/commander/calibration_routines.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/commander/calibration_routines.c')
-rw-r--r--apps/commander/calibration_routines.c219
1 files changed, 0 insertions, 219 deletions
diff --git a/apps/commander/calibration_routines.c b/apps/commander/calibration_routines.c
deleted file mode 100644
index a26938637..000000000
--- a/apps/commander/calibration_routines.c
+++ /dev/null
@@ -1,219 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file calibration_routines.c
- * Calibration routines implementations.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <math.h>
-
-#include "calibration_routines.h"
-
-
-int sphere_fit_least_squares(const float x[], const float y[], const float z[],
- unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
-{
-
- float x_sumplain = 0.0f;
- float x_sumsq = 0.0f;
- float x_sumcube = 0.0f;
-
- float y_sumplain = 0.0f;
- float y_sumsq = 0.0f;
- float y_sumcube = 0.0f;
-
- float z_sumplain = 0.0f;
- float z_sumsq = 0.0f;
- float z_sumcube = 0.0f;
-
- float xy_sum = 0.0f;
- float xz_sum = 0.0f;
- float yz_sum = 0.0f;
-
- float x2y_sum = 0.0f;
- float x2z_sum = 0.0f;
- float y2x_sum = 0.0f;
- float y2z_sum = 0.0f;
- float z2x_sum = 0.0f;
- float z2y_sum = 0.0f;
-
- for (unsigned int i = 0; i < size; i++) {
-
- float x2 = x[i] * x[i];
- float y2 = y[i] * y[i];
- float z2 = z[i] * z[i];
-
- x_sumplain += x[i];
- x_sumsq += x2;
- x_sumcube += x2 * x[i];
-
- y_sumplain += y[i];
- y_sumsq += y2;
- y_sumcube += y2 * y[i];
-
- z_sumplain += z[i];
- z_sumsq += z2;
- z_sumcube += z2 * z[i];
-
- xy_sum += x[i] * y[i];
- xz_sum += x[i] * z[i];
- yz_sum += y[i] * z[i];
-
- x2y_sum += x2 * y[i];
- x2z_sum += x2 * z[i];
-
- y2x_sum += y2 * x[i];
- y2z_sum += y2 * z[i];
-
- z2x_sum += z2 * x[i];
- z2y_sum += z2 * y[i];
- }
-
- //
- //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
- //
- // P is a structure that has been computed with the data earlier.
- // P.npoints is the number of elements; the length of X,Y,Z are identical.
- // P's members are logically named.
- //
- // X[n] is the x component of point n
- // Y[n] is the y component of point n
- // Z[n] is the z component of point n
- //
- // A is the x coordiante of the sphere
- // B is the y coordiante of the sphere
- // C is the z coordiante of the sphere
- // Rsq is the radius squared of the sphere.
- //
- //This method should converge; maybe 5-100 iterations or more.
- //
- float x_sum = x_sumplain / size; //sum( X[n] )
- float x_sum2 = x_sumsq / size; //sum( X[n]^2 )
- float x_sum3 = x_sumcube / size; //sum( X[n]^3 )
- float y_sum = y_sumplain / size; //sum( Y[n] )
- float y_sum2 = y_sumsq / size; //sum( Y[n]^2 )
- float y_sum3 = y_sumcube / size; //sum( Y[n]^3 )
- float z_sum = z_sumplain / size; //sum( Z[n] )
- float z_sum2 = z_sumsq / size; //sum( Z[n]^2 )
- float z_sum3 = z_sumcube / size; //sum( Z[n]^3 )
-
- float XY = xy_sum / size; //sum( X[n] * Y[n] )
- float XZ = xz_sum / size; //sum( X[n] * Z[n] )
- float YZ = yz_sum / size; //sum( Y[n] * Z[n] )
- float X2Y = x2y_sum / size; //sum( X[n]^2 * Y[n] )
- float X2Z = x2z_sum / size; //sum( X[n]^2 * Z[n] )
- float Y2X = y2x_sum / size; //sum( Y[n]^2 * X[n] )
- float Y2Z = y2z_sum / size; //sum( Y[n]^2 * Z[n] )
- float Z2X = z2x_sum / size; //sum( Z[n]^2 * X[n] )
- float Z2Y = z2y_sum / size; //sum( Z[n]^2 * Y[n] )
-
- //Reduction of multiplications
- float F0 = x_sum2 + y_sum2 + z_sum2;
- float F1 = 0.5f * F0;
- float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
- float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
- float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
-
- //Set initial conditions:
- float A = x_sum;
- float B = y_sum;
- float C = z_sum;
-
- //First iteration computation:
- float A2 = A * A;
- float B2 = B * B;
- float C2 = C * C;
- float QS = A2 + B2 + C2;
- float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
-
- //Set initial conditions:
- float Rsq = F0 + QB + QS;
-
- //First iteration computation:
- float Q0 = 0.5f * (QS - Rsq);
- float Q1 = F1 + Q0;
- float Q2 = 8.0f * (QS - Rsq + QB + F0);
- float aA, aB, aC, nA, nB, nC, dA, dB, dC;
-
- //Iterate N times, ignore stop condition.
- int n = 0;
-
- while (n < max_iterations) {
- n++;
-
- //Compute denominator:
- aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
- aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
- aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
- aA = (aA == 0.0f) ? 1.0f : aA;
- aB = (aB == 0.0f) ? 1.0f : aB;
- aC = (aC == 0.0f) ? 1.0f : aC;
-
- //Compute next iteration
- nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
- nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
- nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
-
- //Check for stop condition
- dA = (nA - A);
- dB = (nB - B);
- dC = (nC - C);
-
- if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
-
- //Compute next iteration's values
- A = nA;
- B = nB;
- C = nC;
- A2 = A * A;
- B2 = B * B;
- C2 = C * C;
- QS = A2 + B2 + C2;
- QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
- Rsq = F0 + QB + QS;
- Q0 = 0.5f * (QS - Rsq);
- Q1 = F1 + Q0;
- Q2 = 8.0f * (QS - Rsq + QB + F0);
- }
-
- *sphere_x = A;
- *sphere_y = B;
- *sphere_z = C;
- *sphere_radius = sqrtf(Rsq);
-
- return 0;
-}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-10 09:31:20 +0000