public void AddConstantMatricesSteps(double[,] MeasuresInRows, double[,] StatesInRows, double[,] ForwProp, double[,] BackProp, double[,] MeasCov, double[,] NoisCov,
double[,] StatMeas, double[,] FiltCov)
{
int i, j;
int nm = MeasuresInRows.GetLength(1);
double[] tmpvec = new double[nm];
int ns = StatesInRows.GetLength(1);
double[] tmpvec2 = new double[ns];
int m = MeasuresInRows.GetLength(0);
FilterSteps = new FilterStep[m];
//bisogna manipolare l'indice
for (i = 0; i < m; i++)
{
for (j = 0; j < nm; j++)
{
tmpvec[j] = MeasuresInRows[i, j];
}
for (j = 0; j < ns; j++)
{
tmpvec2[j] = StatesInRows[i, j];
}
FilterSteps[i] = new FilterStep(i, tmpvec, tmpvec2,
ForwProp, BackProp, MeasCov, NoisCov, StatMeas, FiltCov);
}
}
public FilterStep(int index, FilterStep fs)
{
int i, j;
m_Index = index;
m_StateVectorLength = fs.StateVec.Length;
m_MeasurementsVectorLength = fs.MeasCovariance.GetLength(0);
m_MeasureVec = new double[m_MeasurementsVectorLength];
for (i = 0; i < m_MeasurementsVectorLength; i++)
{
m_MeasureVec[i] = fs.MeasureVec[i];
}
m_StateVec = new double[m_StateVectorLength];
m_PredStateVec = new double[m_StateVectorLength];
for (i = 0; i < m_StateVectorLength; i++)
{
m_StateVec[i] = fs.StateVec[i];
}
m_ForwardPropagator = new double[m_StateVectorLength, m_StateVectorLength];
for (i = 0; i < m_StateVectorLength; i++)
{
for (j = 0; j < m_StateVectorLength; j++)
{
m_ForwardPropagator[i, j] = fs.ForwardPropagator[i, j];
}
}
m_BackwardPropagator = new double[m_StateVectorLength, m_StateVectorLength];
for (i = 0; i < m_StateVectorLength; i++)
{
for (j = 0; j < m_StateVectorLength; j++)
{
m_BackwardPropagator[i, j] = fs.BackwardPropagator[i, j];
}
}
m_KalmanGain = new double[m_StateVectorLength, m_MeasurementsVectorLength];
m_StateMeasure = new double[m_MeasurementsVectorLength, m_StateVectorLength];
for (i = 0; i < m_MeasurementsVectorLength; i++)
{
for (j = 0; j < StateVectorLength; j++)
{
m_StateMeasure[i, j] = fs.StateMeasure[i, j];
}
}
m_PredStateCovariance = Matrices.Identity(m_StateVectorLength);
m_FiltStateCovariance = new double[m_StateVectorLength, m_StateVectorLength];
for (i = 0; i < m_StateVectorLength; i++)
{
for (j = 0; j < m_StateVectorLength; j++)
{
m_FiltStateCovariance[i, j] = fs.FiltStateCovariance[i, j];
}
}
m_NoiseCovariance = new double[m_StateVectorLength, m_StateVectorLength];
for (i = 0; i < m_StateVectorLength; i++)
{
for (j = 0; j < m_StateVectorLength; j++)
{
m_NoiseCovariance[i, j] = fs.NoiseCovariance[i, j];
}
}
m_MeasCovariance = new double[m_MeasurementsVectorLength, m_MeasurementsVectorLength];
for (i = 0; i < m_MeasurementsVectorLength; i++)
{
for (j = 0; j < m_MeasurementsVectorLength; j++)
{
m_MeasCovariance[i, j] = fs.MeasCovariance[i, j];
}
}
m_ResidualVec = new double[m_MeasurementsVectorLength];
m_ResidualMat = Matrices.Identity(m_MeasurementsVectorLength);
m_ChiSquared = 0;
}
