private void OnRxEndPointData(object sender, EndpointDataEventArgs e)
{
AddMsg($" > {e.Count} data received");
Array.Copy(e.Buffer, 0, _recvBuf, 0, 8640);
Array.Copy(e.Buffer, 8640, adcBuf, 0, 5120);
int k = 0;
for (int i = 0; i < 5120 - 16; i = i + 16)
{
timerPlotBuf[k] = BitConverter.ToInt16(adcBuf, i);
// timerPlotBuf[k] = BitConverter.ToInt16(e.Buffer, i);
k++;
}
Dispatcher.Invoke(new UpdatePlot(AddPlot), timerPlotBuf);
//Dispatcher.Invoke(new ShowMsg(CreatImg));
for (int i = 0; i < 8640; i = i + 4)
{
_trackImage[i / 4] = BitConverter.ToSingle(_recvBuf, i);
}
for (int i = 8; i < 80; i++)
{
for (int j = 0; j < 27; j++)
{
_matrixDataTemp[i - 8, j] = _trackImage[j + i * 27];
}
}
var result = kalmanFiltering.TraceTableEstablishment(_matrixDataTemp);
if (result.Count > 0)
{
if (kalmanFiltering.StartFlag == 0)
{
var temp = kalmanFiltering.TrackingIni(result);
for (int i = 0; i < 6; i++)
{
kalmanFiltering.PredictPosition[i] = temp[i];
}
trackNum = 0;
}
else
{
kalmanFiltering.Tracking(result);
xTemp[trackNum] = kalmanFiltering.CurrentPosition[0] * 100;
yTemp[trackNum] = kalmanFiltering.CurrentPosition[3] * 100;
trackNum++;
}
}
else
{
if (kalmanFiltering.TrackCount > 0)
{
kalmanFiltering.MissCount++;
}
if (kalmanFiltering.MissCount > 50)
{
double[] plotTemp = new double[kalmanFiltering.TrackCount * 2];
for (int i = 0; i < kalmanFiltering.TrackCount; i++)
{
plotTemp[i] = xTemp[i];
plotTemp[i + kalmanFiltering.TrackCount] = yTemp[i];
}
//Dispatcher.Invoke(new UpdateTrack(AddTrack), plotTemp);
trackNum = 0;
kalmanFiltering.TrackCount = 0;
kalmanFiltering.StartFlag = 0;
}
}
if (_dataState == DataState.Started)
{
Dispatcher.Invoke(new UpdateBytesDelegate(SaveData), adcBuf);
}
}
public void KalmanFilteringTest()
{
var vectorBuilder = Vector <double> .Build;
var matrixBuild = Matrix <double> .Build;
var resultPositon = vectorBuilder.Dense(6, 0);
resultPositon[0] = -0.11712893894299961;
resultPositon[1] = -0.0085413873210193024;
resultPositon[2] = -0.00034048259997198428;
resultPositon[3] = 0.47421163145835582;
resultPositon[4] = -0.00690331375794314;
resultPositon[5] = -0.00027109000134432605;
double[,] prevariance =
{
{
0.242766594755489, 0.0523105449659795, 0.00101097341848172, 7.62628147622445e-09,
1.83912407997933e-06, 0.000104585919975806
},
{
0.0523105449659795, 7.99334660284189, 0.220587578642543, 1.83912407997933e-06, 0.000460038020961047,
0.0287783990337413
},
{
0.00101097341848172, 0.220587578642543, 10.3999959410456, 0.000104585919975806, 0.0287783990337413,
2.39999917381918
},
{
7.62628147622445e-09, 1.83912407997933e-06, 0.000104585919975806, 0.242766594755489,
0.0523105449659795, 0.00101097341848172
},
{
1.83912407997933e-06, 0.000460038020961047, 0.0287783990337413, 0.0523105449659795,
7.99334660284189, 0.220587578642543
},
{
0.000104585919975806, 0.0287783990337413, 2.39999917381918, 0.00101097341848172, 0.220587578642543,
10.3999959410456
}
};
var varianceTemp = matrixBuild.DenseOfArray(prevariance);
var m1 = MatlabReader.ReadAll <double>("testData.mat");
KalmanFiltering kalmanFiltering = new KalmanFiltering();
foreach (Matrix <double> testData in m1.Values)
{
var result = kalmanFiltering.TraceTableEstablishment(testData);
kalmanFiltering.PrePosition[0] = -0.0950698731643758;
kalmanFiltering.PrePosition[1] = 0.00483652853477623;
kalmanFiltering.PrePosition[2] = 9.61518040733592e-05;
kalmanFiltering.PrePosition[3] = 0.487928863356650;
kalmanFiltering.PrePosition[4] = 0.00140521891949567;
kalmanFiltering.PrePosition[5] = 3.61578884580855e-05;
kalmanFiltering.PreVarience = varianceTemp;
kalmanFiltering.Tracking(result);
Assert.AreEqual(resultPositon, kalmanFiltering.CurrentPosition);
}
}