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 DoFilter(double noisyXPosition, GameObject broca)
{
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);*/
}
}