public static double Simulate(Specimen spec) { var fkf = spec.ToFedKf(); var meas = new double[4]; var n = DirectCosinsMatrix; //var nt = n.Transpose(); //var c = SensorsOutputCovariances.Inverse(); //var pseudoInverse = (nt * n).Inverse() * nt; var err = 0.0; int lng = Math.Min(Signals.RowCount, MaxSimLength); var results = new Vector3[lng]; results[0] = new Vector3 { X = 0.0, Y = 0.0, Z = 0.0 }; for (int i = 0; i < lng; i++) { var sigRow = Signals.Row(i); var noiseRow = Noises.Row(i); var targRow = Targets.Row(i); meas[0] = sigRow[0] + noiseRow[0]; meas[1] = sigRow[1] + noiseRow[1]; meas[2] = sigRow[2] + noiseRow[2]; meas[3] = sigRow[3] + noiseRow[3]; DenseMatrix inps; if (i > 0) { inps = n * results[i - 1].ToMatrix(); } else { inps = new DenseMatrix(4, 1, 0.0); } var res = fkf.Step(meas, inps.ToColumnWiseArray()); var errs = new double[] { res[0, 0] - targRow[0], res[1, 0] - targRow[1], res[2, 0] - targRow[2] }; err += (errs[0] * errs[0]) + (errs[1] * errs[1]) + (errs[2] * errs[2]); results[i] = new Vector3 { X = res[0, 0], Y = res[1, 0], Z = res[2, 0] }; if (PrintSimResults) { Console.WriteLine(res.ToColumnWiseArray().Print()); } if (double.IsNaN(err)) { return(double.NaN); } } if (PrintSimResults) { FileParser.Write3ColonFile(@"Data\Evaluations1000.csv", results); } return(1 / err * lng); }