public override decimal[] calibrate() { Func <decimal[], decimal[, ]> J = new Func <decimal[], decimal[, ]>(computeJ); Func <decimal[], decimal[]> r = new Func <decimal[], decimal[]>(computeR); decimal[] p = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; decimal[,] M, Mfinal = new decimal[3, 3] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } }; decimal[] b, bFinal = new decimal[3] { 0, 0, 0 }; for (int j = 0; j < 10; j++) { p = Optimization.GaussNewton(J, r, p); M = new decimal[, ] { { p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] } }; b = new decimal[] { p[9], p[10], p[11] }; Mfinal = MathDecimal.Prod(Mfinal, M); bFinal = MathDecimal.Sum(MathDecimal.Prod(M, bFinal), b); for (int i = 0; i < data.Count; i++) { data[i].data = MathDecimal.Sum(b, MathDecimal.Prod(M, data[i].data)); } //scale with respect to the gravitational acceleration /*decimal[] rGravity = new decimal[data.Count]; * for (int i = 0; i < rGravity.GetLength(0); i++) * rGravity[i] = Preferences.G; * decimal[,] M = new decimal[,] { { p[0], p[1], p[2] }, { p[3], p[4], p[5] }, { p[6], p[7], p[8] } }; * decimal[] b = new decimal[] { p[9], p[10], p[11] }; * decimal[,] JGravity = new decimal[data.Count,1]; * for (int i = 0; i < rGravity.GetLength(0); i++) * JGravity[i,1] = MathDecimal.Norm2(MathDecimal.Sum(MathDecimal.Prod(M, data[i].data),b)); * decimal Q = Optimization.LinearLeastSquares(JGravity, rGravity)[1]; * p = MathDecimal.Prod(Q, p);*/ } pars = new CalibrationParameter(Mfinal[0, 0], Mfinal[0, 1], Mfinal[0, 2], Mfinal[1, 0], Mfinal[1, 1], Mfinal[1, 2], Mfinal[2, 0], Mfinal[2, 1], Mfinal[2, 2], bFinal[0], bFinal[1], bFinal[2]); return(p); }
public Accelerometer(List <Measurement> data, CalibrationParameter pars = null) : base(data, pars) { }
public Sensor(List <Measurement> data, CalibrationParameter pars = null) { this.data = data; this.pars = pars; }