private void SetKalmanFilter()
{
int j, n;
n = m_Track.Length;
int ng;
kf = new KalmanFilter();
double[] mrows = new double[4];
for (j = 0; j < n; j++)
{
mrows[0] = m_Track[j].Info.Intercept.X;
mrows[1] = m_Track[j].Info.Slope.X;
mrows[2] = m_Track[j].Info.Intercept.Y;
mrows[3] = m_Track[j].Info.Slope.Y;
if (j == n - 1)
{
ng = 1;
}
else
{
ng = Math.Abs(m_Track[j + 1].LayerOwner.SheetId - m_Track[j].LayerOwner.SheetId);
}
m_fp = new double[4, 4] {
{ 1, -ng * m_ProjectionZ, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, -ng * m_ProjectionZ }, { 0, 0, 0, 1 }
};
m_bp = new double[4, 4] {
{ 1, ng *m_ProjectionZ, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, ng *m_ProjectionZ }, { 0, 0, 0, 1 }
};
FilterStep fs = new FilterStep(j, mrows, mrows, m_fp, m_bp, m_mc, m_nc, m_st, m_fc);
kf.AddStep(fs);
}
}
private void SetKalmanFilter()
{
int j, n;
n = m_Track.Length;
double projz;
kf = new KalmanFilter();
double[] srows = new double[6];
double[] mrows = new double[5];
for (j = 0; j < n; j++)
{
srows[0] = m_Track[j].Info.Intercept.X;
srows[1] = m_Track[j].Info.Slope.X;
srows[2] = m_Track[j].Info.Intercept.Y;
srows[3] = m_Track[j].Info.Slope.Y;
srows[4] = m_Track[j].Info.Intercept.Z;
srows[5] = 1;
mrows[0] = srows[0];
mrows[1] = srows[1];
mrows[2] = srows[2];
mrows[3] = srows[3];
mrows[4] = srows[4];
if (j == n - 1)
{
projz = Math.Abs(m_Track[j].Info.Intercept.Z - m_Track[j - 1].Info.Intercept.Z);
}
else
{
projz = Math.Abs(m_Track[j + 1].Info.Intercept.Z - m_Track[j].Info.Intercept.Z);
}
m_fp = new double[6, 6] {
{ 1, -projz, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 }, { 0, 0, 1, -projz, 0, 0 }, { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, -projz }, { 0, 0, 0, 0, 0, 1 }
};
m_bp = new double[6, 6] {
{ 1, projz, 0, 0, 0, 0 }, { 0, 1, 0, 0, 0, 0 }, { 0, 0, 1, projz, 0, 0 }, { 0, 0, 0, 1, 0, 0 }, { 0, 0, 0, 0, 1, projz }, { 0, 0, 0, 0, 0, 1 }
};
FilterStep fs = new FilterStep(j, mrows, srows, m_fp, m_bp, m_mc, m_nc, m_st, m_fc);
kf.AddStep(fs);
}
}
