private Matrix CovarianceEstimation(Matrix kalmanGain) { var KH = (kalmanGain * H); var khOrder = KH.ColumnCount; return((Matrix.Identity(khOrder) - KH) * P); }
public void Test_EstimatePolynomial() { // Polynomial a(x-r0)(x-r1)... int n = 8; float[] x = new float[] { -1.2f, 1.2f, 1.4f, 1.6f, 1.8f, 2.2f, -1.6f, -1.4f }; Matrix<float> estimationMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(n, 2); for(int r = 0; r < n; ++r) { estimationMatrix.At(r, 0, x[r]); estimationMatrix.At(r, 1, (float)PolyValue(x[r])); } Polynomial poly = Polynomial.EstimatePolynomial(estimationMatrix, rank); // Check if values are same for extimated coeffs and real poly double[] testX = new double[] { 1.01, -10.0, 100.0 }; double real0 = PolyValue(testX[0]); double est0 = poly.At((float)testX[0]); double real1 = PolyValue(testX[1]); double est1 = poly.At((float)testX[1]); double real2 = PolyValue(testX[2]); double est2 = poly.At((float)testX[2]); Assert.IsTrue(Math.Abs(PolyValue(testX[0]) / poly.At((float)testX[0]) - 1.0) < 1e-3); Assert.IsTrue(Math.Abs(PolyValue(testX[1]) / poly.At((float)testX[1]) - 1.0) < 1e-3); Assert.IsTrue(Math.Abs(PolyValue(testX[2]) / poly.At((float)testX[2]) - 1.0) < 1e-3); }
public void Test_FindRoots_EstimatedPoly() { int n = 8; float[] x = new float[] { -1.2f, 1.2f, 1.4f, 1.6f, 1.8f, 2.2f, -1.6f, -1.4f }; Matrix<float> estimationMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(n, 2); for(int r = 0; r < n; ++r) { estimationMatrix.At(r, 0, x[r]); estimationMatrix.At(r, 1, (float)PolyValue(x[r])); } Polynomial poly = Polynomial.EstimatePolynomial(estimationMatrix, rank); PolynomialRootFinder rootFinder = new PolynomialRootFinder(); rootFinder.Poly = poly; rootFinder.Process(); var roots = rootFinder.RealRoots; Assert.IsTrue(roots.Count == rank); Array.Sort(_r); roots.Sort(); for(int i = 0; i < rank; ++i) { Assert.IsTrue( Math.Abs(roots[i] / _r[i] - 1.0f) < 1e-4f || Math.Abs(roots[i] - _r[i]) < 1e-4f); } }
static Matrix QGenerator(float dt, float stdDevProcessNoise) { // B * B^T * stdDev_w ^ 2 var dt2 = dt * dt; var dt3 = dt2 * dt; var BBT = Matrix.OfArray(new float[, ] { { 0.25f * dt2 * dt2, 0.5f * dt3 }, { 0.5f * dt3, dt2 } }); return(BBT * stdDevProcessNoise * stdDevProcessNoise); }
public KalmanFilter(KalmanSetup setup) { this.stdDevProcess = setup.StdDevProcess; this.stdDevMeasurement = setup.StdDevMeasurement; this.X = setup.X0; this.P = setup.P0; this.H = setup.H; this.F = setup.funcF; this.B = setup.funcB; this.Q = setup.funcQ; this.R = setup.funcR(stdDevMeasurement); }
public Vector Update(float dt, Vector measurement, Vector controlInput) { // Predict this.X = PredictState(dt, controlInput); this.P = PredictCovariance(dt); // Update var y = MeasurementResidual(measurement); var S = CovarianceResidual(); var K = KalmanGain(S); this.X = StateEstimation(K, y); this.P = CovarianceEstimation(K); return(this.X); }
private Vector CrossProduct(Vector u, Vector v) { var iArr = new float[, ] { { u[1], u[2] }, { v[1], v[2] } }; var jArr = new float[, ] { { u[2], u[0] }, { v[2], v[0] } }; var kArr = new float[, ] { { u[0], u[1] }, { v[0], v[1] } }; var i = DMatrix.OfArray(iArr); var j = DMatrix.OfArray(jArr); var k = DMatrix.OfArray(kArr); var nArr = new float[] { i.Determinant(), j.Determinant(), k.Determinant() }; return(new DVector(nArr)); }
private ulong onePointKnn( DenseColumnMajorMatrixStorage <float> query, int i, DenseColumnMajorMatrixStorage <int> indices, DenseColumnMajorMatrixStorage <float> dists2, Vector <float> maxRadii, int k, float epsilon, SearchOptionFlags optionFlags) { var results = new ListPriorityQueue <int>(maxSize: k); var queryMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(query); var q = queryMatrix.Column(i); for (int j = 0; j < cloud.ColumnCount; j++) { var c = cloud.Column(j); var diff = (c - q); float l2 = diff * diff; results.Enqueue(j, l2); } int kIdx = 0; foreach (var j in results.Items) { indices.At(kIdx, i, j); kIdx++; } kIdx = 0; foreach (var d2 in results.Priorities) { dists2.At(kIdx, i, d2); } return(0); }
private Vector StateEstimation(Matrix kalmanGain, Vector measurementResidual) { return(X + kalmanGain * measurementResidual); }
private Matrix KalmanGain(Matrix covarianceResidual) { return(P * (Matrix)(H.Transpose() * covarianceResidual.Inverse())); }
public Field4Drepresentation( DenseMatrix dmListOfData, Dictionary <string, object> properties, Dictionary <string, int> valuesColumnsIndeces = null, SlicingVariable varSliceBy = SlicingVariable.z, string description = "") { InitializeComponent(); variableSliceBy = varSliceBy; if (valuesColumnsIndeces == null) { valuesColumnsIndeces = new Dictionary <string, int>(); valuesColumnsIndeces.Add("x", 0); valuesColumnsIndeces.Add("y", 1); valuesColumnsIndeces.Add("z", 2); valuesColumnsIndeces.Add("values", 3); } strDataDescription = description; ThreadSafeOperations.SetText(lblDescription, strDataDescription, false); defaultProperties = properties; strOutputDirectory = (string)defaultProperties["DefaultDataFilesLocation"]; if (!ServiceTools.CheckIfDirectoryExists(strOutputDirectory)) { strOutputDirectory = ""; } dmDataList = dmListOfData.Copy(); double dataMaxVal = dmDataList.Column(3).Max(); double dataMinVal = dmDataList.Column(3).Min(); ILScene scene = new ILScene(); currSurfPlotCube = new ILPlotCube(); currSurfPlotCube.TwoDMode = false; List <List <DenseMatrix> > lDataMatricesForSurfices = ReshapeDataToMatrices(dmDataList, variableSliceBy); foreach (List <DenseMatrix> currSurfMatricesList in lDataMatricesForSurfices) { ILInArray <double> ilaXvalues = currSurfMatricesList[0].ToArray(); ILInArray <double> ilaYvalues = currSurfMatricesList[1].ToArray(); ILInArray <double> ilaZvalues = currSurfMatricesList[2].ToArray(); MathNet.Numerics.LinearAlgebra.Single.DenseMatrix floatDMcolors = MathNet.Numerics.LinearAlgebra.Single.DenseMatrix.Create(currSurfMatricesList[0].RowCount, currSurfMatricesList[0].ColumnCount, 0.0f); currSurfMatricesList[3].MapConvert <Single>(dval => Convert.ToSingle(dval), floatDMcolors, MathNet.Numerics.LinearAlgebra.Zeros.Include); ILInArray <float> ilaForColorValues = floatDMcolors.ToArray(); ILSurface currSurf = new ILSurface(ilaZvalues, ilaXvalues, ilaYvalues); currSurf.ColorMode = ILSurface.ColorModes.RBGA; ColorScheme newCS = new ColorScheme(""); //ColorSchemeRuler newCSR = new ColorSchemeRuler(newCS, dataMinVal, dataMaxVal); double[,] dmRvalues = DenseMatrix.Create(floatDMcolors.RowCount, floatDMcolors.ColumnCount, (r, c) => { Bgr currColor = newCS.GetColorByValueAndRange(currSurfMatricesList[3][r, c], dataMinVal, dataMaxVal); return(currColor.Red / 255.0d); }).ToArray(); double[,] dmGvalues = DenseMatrix.Create(floatDMcolors.RowCount, floatDMcolors.ColumnCount, (r, c) => { Bgr currColor = newCS.GetColorByValueAndRange(currSurfMatricesList[3][r, c], dataMinVal, dataMaxVal); return(currColor.Green / 255.0d); }).ToArray(); double[,] dmBvalues = DenseMatrix.Create(floatDMcolors.RowCount, floatDMcolors.ColumnCount, (r, c) => { Bgr currColor = newCS.GetColorByValueAndRange(currSurfMatricesList[3][r, c], dataMinVal, dataMaxVal); return(currColor.Blue / 255.0d); }).ToArray(); float[, ,] rgbaArrData = new float[4, dmRvalues.GetLength(0), dmRvalues.GetLength(1)]; for (int i = 0; i < dmRvalues.GetLength(0); i++) { for (int j = 0; j < dmRvalues.GetLength(1); j++) { rgbaArrData[0, i, j] = Convert.ToSingle(dmRvalues[i, j]); rgbaArrData[1, i, j] = Convert.ToSingle(dmGvalues[i, j]); rgbaArrData[2, i, j] = Convert.ToSingle(dmBvalues[i, j]); rgbaArrData[3, i, j] = 0.3f; } } currSurf.UpdateRGBA(null, rgbaArrData); currSurf.UseLighting = true; currSurfPlotCube.Children.Add(currSurf); //if (currSurfPlotCube.Children.Count() > 4) //{ // currSurfPlotCube.Children.Add(new ILColorbar()); //} //if (currSurfPlotCube.Children.Count() > 4) //{ // break; //} } // surf.Children.Add(new ILColorbar()); //currSurfPlotCube.Children.Add(new ILColorbar()); //currSurfPlotCube.Rotation = Matrix4.Rotation(new Vector3(1, 0, 0), 1.2f) * // Matrix4.Rotation(new Vector3(0, 0, 1), Math.PI); currSurfPlotCube.Projection = Projection.Orthographic; currSurfPlotCube.Plots.Reset(); scene.Add(currSurfPlotCube); ilPanel1.Scene = scene; }
public static async Task <PointCloud> GenerateObject(Recording recording, IProgress <int> progress, CancellationToken ct) { // Quick access image formats var dFormat = recording.DepthFormat; var cFormat = recording.ColourFormat; #region Check for calibration data if (!recording.IsCalibratedRecording) { throw new IOException("Pre-calibrated recording required!"); } var calibration = recording.Calibration; #endregion #region Kalman Filter setup var kSetup = new KalmanSetup() { funcF = new Func <float, Matrix>(FGenerator), funcB = new Func <float, Matrix>(BGenerator), H = Matrix.OfArray(new float[, ] { { 1, 0 } }), X0 = new Vector(new float[] { 0, 0 }), P0 = Matrix.OfArray(new float[, ] { { 10, 0 }, { 0, 10 } }), funcQ = new Func <float, float, Matrix>(QGenerator), funcR = new Func <float, Matrix>(RGenerator), StdDevMeasurement = stdDevMeasurementNoise, StdDevProcess = stdDevProcessNoise }; float R = calibration.TurntableRadius; float acc = -4 * mu * g / (3 * R); var accelerationVector = new Vector(new float[] { acc }); #endregion #region Initialise pipeline stages var cv = new HueThresholder(calibration.Markers, calibration.MarkerSearchSpace); var geom = new PointCloudBuilder(calibration, recording.MappingParameters); var kf = new KalmanFilter(kSetup); var opt = new ICPMinimiser(true); #endregion // start new pipeline thread return(await Task.Run <PointCloud>(() => { var returnCloud = new PointCloud(); PointCloud prevCloud = null; float lastTime = 0; double theta = 0; Point3D lastx = new Point3D(0, 0, 0); int advanceUnit = Processing.Settings.Default.ProcessEveryFrame ? 1 : 10; #region KF Debug Logging StreamWriter filterLog = null; if (DebugSettings.Default.DebugMode) { filterLog = new StreamWriter(DebugSettings.Default.LogRoot + @"\filter.csv"); filterLog.AutoFlush = true; filterLog.WriteLine("t,dt,dx,dtheta,theta,theta_KF,omega_KF"); } #endregion for (int i = 0; i < recording.NumberOfFrames; i += advanceUnit) { ct.ThrowIfCancellationRequested(); progress.Report(i); #region Fetch // Fetch frame from disk float timestamp = recording.Timestamps[i]; var colour = recording.FetchColourFrame(i); var depth = recording.FetchDepthFrame(i); Frame f = new Frame(i, timestamp, depth, dFormat, colour, cFormat); #endregion #region Analyse (CV) var markers2D = cv.IdentifyMarkers(f); if (markers2D.Count == 0) { i -= (advanceUnit - 1); continue; } #endregion #region Build Geometry (GEOM) PointCloud cloud; KeyValuePair <TrackingMarker, Point3D> rotationMarker; var geomSuccess = geom.ConstructCloud(f, markers2D, out cloud, out rotationMarker); if (!geomSuccess) { i -= (advanceUnit - 1); continue; } #endregion #region Filter // reposition marker based on offset var polarX = CoordinateSystemConvertor.ToSpherical(rotationMarker.Value); polarX.Azimuthal += (float)rotationMarker.Key.RotationalOffset; var x = CoordinateSystemConvertor.ToCartesian(polarX); // rotation approximation var dx = ((Vector)x - lastx).Norm(3); var dtheta = dx / calibration.TurntableRadius; // update cumulative angle (with estimated angle) theta += dtheta; // calculate time delta float dt = f.Timestamp - lastTime; if (DebugSettings.Default.DebugMode) { filterLog.Write("{0},{1},{2},{3},{4},", f.Timestamp, dt, dx, dtheta, theta); } // calculate state vector var stateVector = new Vector(new float[] { (float)theta }); // pass through kalman filter var filteredStateVector = kf.Update(dt, stateVector, accelerationVector); // extract angle from state vector var filteredTheta = filteredStateVector[0]; var filteredOmega = filteredStateVector[1]; if (DebugSettings.Default.DebugMode) { filterLog.WriteLine("{0},{1}", filteredTheta, filteredOmega); } // update last time, last position and set filtered cumulative theta theta = filteredTheta; lastx = x; lastTime = f.Timestamp; #endregion #region Optimise (OPT) PointCloud optCloud = null; if (prevCloud == null) { optCloud = cloud; // first frame; this is requires no optimising transformation } else if (prevCloud.Points.Count == 0) { optCloud = cloud; // no points in previous cloud (model); cannot align the current cloud to model } else { // Build best guess transformation matrix float sinT = (float)Math.Sin(filteredTheta); float cosT = (float)Math.Cos(filteredTheta); var bestGuessTransformation = Matrix.Identity(4); bestGuessTransformation[0, 0] = cosT; bestGuessTransformation[0, 2] = -sinT; bestGuessTransformation[2, 0] = sinT; bestGuessTransformation[2, 2] = cosT; // returned object is result of aligning cloud with prevCloud optCloud = opt.Process(cloud, prevCloud, bestGuessTransformation); } prevCloud = cloud; #endregion #region Retire returnCloud.Points.AddRange(optCloud.Points); #endregion } if (DebugSettings.Default.DebugMode) { filterLog.Close(); } return returnCloud; })); }
static Matrix RGenerator(float stdDevMeasurementNoise) { return(Matrix.OfArray(new float[, ] { { stdDevMeasurementNoise *stdDevMeasurementNoise } })); }
static Matrix BGenerator(float dt) { return(Matrix.OfArray(new float[, ] { { 0.5f * dt * dt }, { dt } })); }
static Matrix FGenerator(float dt) { return(Matrix.OfArray(new float[, ] { { 1, dt }, { 0, 1 } })); }
void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { /*do nothing*/ //pull frame from skeleton -- display it using (BodyFrame bf = e.FrameReference.AcquireFrame()) { if (bf == null) return; //do nothing if nothing found bf.GetAndRefreshBodyData(bStorage); //copy given skeleton(s) frames (if multiple skeletons found) into buffer //once the closest skeleton is determined draw THAT stickman //pointer to closest skeleton frame Body closestBody = null; // int closestId = -1; double closestDistance = double.MaxValue; //out of detected skeletons in frame use the closest one -- determine closest one here foreach (var body in bStorage) { //if valid skeleton if (body.IsTracked) { if (body.Joints[JointType.SpineShoulder].Position.Z < closestDistance) { //update Id and distance // closestId = 0; closestBody = body; //set this skeleton as closest closestDistance = body.Joints[JointType.SpineShoulder].Position.Z; } } } //draw the closest skeleton IF found if (closestBody != null) { //draw next //always clear cavnas StickMen.Children.Clear(); DrawStickMan(closestBody, Brushes.Black, 3); } else { return; //nothing found! } //DrawStickMan(Skeleton skeleton, Brush brush, int thickness) //if progress bar has filled if (timerCount == 0) { //recording logic if (currentState == ScreenStates.RECORDING_ENROLL || currentState == ScreenStates.RECORDING_LOGIN_GIVEN || currentState == ScreenStates.RECORDING_LOGIN_GUESS) { //quickly allocate space -- rely on garbage collection huehuehue float[] currJoints = new float[numJoints * 3]; for (int i = 0; i < jointorder.Length; i++) { currJoints[i * 3] = closestBody.Joints[(JointType)jointorder[i]].Position.X; currJoints[i * 3 + 1] = closestBody.Joints[(JointType)jointorder[i]].Position.Y; currJoints[i * 3 + 2] = closestBody.Joints[(JointType)jointorder[i]].Position.Z; } if (rb.getBufferStatus() == true) //recording complete AND/OR buffer-size was reached { if ((currentState == ScreenStates.RECORDING_ENROLL)) { float[,] lookupData, queryDataExpanded, queryData; queryDataExpanded = (rb.getBufferData()); queryData = new float[rb.getBufferNumberFrames(), numJoints * 3]; for (int i = 0; i < rb.getBufferNumberFrames(); i++) { for (int j = 0; j < numJoints * 3; j++) { queryData[i, j] = queryDataExpanded[i, j]; } } queryData = normalizeData(queryData); dataStore[currentUser][0][recordNumber] = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(queryData); if ((recordNumber != numRecords - 1)) { recordNumber++; rb.clearBuffer(); startTimer(3); setProgramState(ScreenStates.RECORDING_ENROLL); } else { //store in g1, s1 slot dataEnrolled = true; dataStore[currentUser][0][recordNumber] = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(queryData); userHasData[currentUser] = true; showPopup("Recorded " + rb.getBufferNumberFrames() + " frames for " + userNames[currentUser], Brushes.Blue); //Code for saving dataStore as .csv file //For each user Directory.CreateDirectory(desktopDir + "KinectSamples\\User" + (currentUser + 1).ToString()); //For each Sample for (int sNum = 0; sNum < numRecords; sNum++) { using (System.IO.StreamWriter file = new System.IO.StreamWriter(@desktopDir + "KinectSamples\\User" + (currentUser + 1).ToString() + "\\Sample" + sNum.ToString() + ".csv", false)) { for (int j = 0; j < dataStore[currentUser][0][sNum].RowCount; j++) { for (int i = 0; i < numJoints * 3 - 1; i++) { file.Write(Convert.ToString(dataStore[currentUser][0][sNum][j, i]) + ","); } file.Write(Convert.ToString(dataStore[currentUser][0][sNum][j, numJoints * 3 - 1]) + "\n"); } } } setProgramState(ScreenStates.HOME); } } else { //bad "formatting" float[,] lookupData, queryDataExpanded, queryData; float bestScore = float.PositiveInfinity; float dtwScore = float.PositiveInfinity; float[][,] saveData = new float[5][,]; int userMatchIdx = -1; //hide skeleton while processing StickMen.Visibility = System.Windows.Visibility.Hidden; queryDataExpanded = (rb.getBufferData()); queryData = new float[rb.getBufferNumberFrames(), numJoints * 3]; for (int i = 0; i < rb.getBufferNumberFrames(); i++) { for (int j = 0; j < numJoints * 3; j++) { queryData[i, j] = queryDataExpanded[i, j]; } } queryData = normalizeData(queryData); //compute distances switch (currentState) { case ScreenStates.RECORDING_LOGIN_GIVEN: for (int i = 0; i < numRecords; i++) { lookupData = dataStore[currentUser][0][i].ToArray(); dtwScore = gestureDistance(queryData, lookupData, queryData.GetLength(0), lookupData.GetLength(0)); //TODO some analysis here ahahahahah ResultText.Text = "User: "******"|Score: " + dtwScore; if (dtwScore <= acceptThreshold) { showPopup("Welcome back, " + userNames[currentUser], Brushes.Green); break; } else if (i == numRecords - 1) { showPopup("Sorry, I do not think you are " + userNames[currentUser], Brushes.Red); } } setProgramState(ScreenStates.HOME); //setProgramState(ScreenStates.RESULT); break; case ScreenStates.RECORDING_LOGIN_GUESS: for (int uIdx = 0; uIdx < userHasData.Length; uIdx++) { if (userHasData[uIdx] == false) continue; for (int i = 0; i < numRecords; i++) { lookupData = dataStore[uIdx][0][i].ToArray(); dtwScore = gestureDistance(queryData, lookupData, queryData.GetLength(0), lookupData.GetLength(0)); if (dtwScore < bestScore) { bestScore = dtwScore; userMatchIdx = uIdx; } } } //after ALL comparisons then do some result feeding here //TODO ResultText.Text = "User:"******"|Score:" + bestScore; if (bestScore <= acceptThreshold) { showPopup("Welcome back, " + userNames[userMatchIdx], Brushes.Green); } else { showPopup("Sorry, I do not think you are anyone I know.", Brushes.Red); } setProgramState(ScreenStates.HOME); //ResultText.Text = //setProgramState(ScreenStates.RESULT); break; default: break; } } } else { rb.addFrame(currJoints, recordNumber); StickMen.Children.Clear(); //update GUI to reflect recording or status here if (rb.isMoving()) { DrawStickMan(closestBody, Brushes.Green, 3); } else { DrawStickMan(closestBody, Brushes.Black, 3); } } } } } }