void Update()
{
filter.PredictState(Time.deltaTime);
//Dummy Data : Generate a very noisy position estimate
noisyXPosition = Mathf.Sin(Time.time) + Random.Range(-0.5f, 0.5f);
filter.UpdateState(new double[, ]
{
{ noisyXPosition }, //Noisy Position
{ 0.0 },
{ 0.0 },
{ 0.0 },
{ 0.0 },
{ 0.0 },
{ -Mathf.Sin(Time.time) }, //Flawless acceleration
{ 0.0 },
{ 0.0 }
});
double oldPos = filter.StateMatrix[0, 0];
for (int i = 1; i < 10; i++)
{
double[,] PredictedState = filter.SafePredictState(0.1 * i);
Debug.DrawLine(new Vector3((float)oldPos, i * 0.02f, 0f), new Vector3((float)PredictedState[0, 0], (i + 1) * 0.02f, 0f), Color.green);
oldPos = PredictedState[0, 0];
Debug.DrawLine(new Vector3(Mathf.Sin((Time.time + (i * 0.1f))), i * 0.02f, 0f),
new Vector3(Mathf.Sin((Time.time + ((i + 1f) * 0.1f))), (i + 1) * 0.02f, 0f), Color.red);
}
}
void Update()
{
noisyXPosition = broca.transform.position.x;
noisyYPosition = broca.transform.position.y;
noisyZPosition = broca.transform.position.z;
filter.PredictState(Time.deltaTime);
DoFilter(noisyXPosition, broca);
DoFilter(noisyYPosition, broca);
DoFilter(noisyZPosition, broca);
//Dummy Data : Generate a very noisy position estimate
}
public Tuple <Matrix <double>, Matrix <double> > Predict(TimeSpan ts)
{
return(KalmanFilter.PredictState(ts, F, G, Q));
}