public ProccessedTrack ProccessTrack(Track track)
{
ProccessedTrack pTrack = new ProccessedTrack("Фильтр Калмана",false);
pTaL = new double[3];
InitializeP();
NormalRandom normRand = new NormalRandom();
aX = Parameters[0].DoubleValue;
aY = Parameters[1].DoubleValue;
r = Parameters[2].DoubleValue;
double predLatForAdd = 0;
double predLonForAdd = 0;
for (int i = 0; i < track.TrackItems.Length; i++)
{
DateTime date = track.TrackItems[i].Date;
double[] kTal = new double[2];
for (int j = 0; j < 2; j++)
{
kTal[j] = pTaL[j] / (pTaL[j] + r);
/*pTaL[j] = (1 - kTal[j]) * pTaL[j];*/
pTaL[j] -= (pTaL[j] > r) ? r : 0;
}
double predLat = predLatForAdd + aX * normRand.Next();
double predLon = predLonForAdd + aY * normRand.Next();
predLatForAdd = predLat + kTal[0] * (track.TrackItems[i].Latitude - predLat);
predLonForAdd = predLon + kTal[1] * (track.TrackItems[i].Longitude - predLon);
ProccessedTrackItem temp = new ProccessedTrackItem(track.TrackItems[i],predLatForAdd, predLonForAdd);
pTrack.AddProccessedTrackItem(temp);
}
return pTrack;
}
/// <summary>
/// получение обработанного трека
/// </summary>
/// <param name="track">Исходный трек</param>
/// <returns>Обработанный трек</returns>
public ProccessedTrack ProccessTrack(Track track)
{
ProccessedTrack pTrack = new ProccessedTrack(Message);
avg = Parameters[0].IntegerValue;
TrackItem[] trackItems = track.TrackItems;
for (int i = 0; i < avg / 2; i++)
{
ProccessedTrackItem pItem = new ProccessedTrackItem(trackItems[i], trackItems[i].Latitude, trackItems[i].Longitude);
pTrack.AddProccessedTrackItem(pItem);
}
for (int i = avg / 2; i < trackItems.Length - avg / 2; i++)
{
double sumLat = 0;
double sumLon = 0;
for (int j = i - avg / 2; j <= i + avg / 2; j++)
{
sumLat += trackItems[j].Latitude;
sumLon += trackItems[j].Longitude;
}
sumLat /= avg + (avg + 1) % 2;
sumLon /= avg + (avg + 1) % 2;
ProccessedTrackItem pItem = new ProccessedTrackItem(trackItems[i], sumLat, sumLon);
pTrack.AddProccessedTrackItem(pItem);
}
for (int i = trackItems.Length - avg / 2; i < trackItems.Length; i++)
{
ProccessedTrackItem pItem = new ProccessedTrackItem(trackItems[i], trackItems[i].Latitude, trackItems[i].Longitude);
pTrack.AddProccessedTrackItem(pItem);
}
return pTrack;
}
public ProccessedTrack ProccessTrack(Track track)
{
ProccessedTrack pTrack = new ProccessedTrack(Message, false);
foreach (TrackItem item in track.TrackItems)
{
double[] vec = new double[] {item.Longitude,item.Latitude };
double[] newVec = CalcEstimation(vec);
ProccessedTrackItem pItem = new ProccessedTrackItem(item, newVec[1], newVec[0]);
pTrack.AddProccessedTrackItem(pItem);
}
return pTrack;
}
/// <summary>
/// Добавить обработанную координату
/// </summary>
/// <param name="item">Обработанная координата</param>
public void AddProccessedTrackItem(ProccessedTrackItem item)
{
processedTrackItems.Add(item);
}
/// <summary>
/// обработать таблицу с обработанным треком
/// </summary>
/// <param name="dataTable">Таблица</param>
/// <returns>Обработанный трек</returns>
private ProccessedTrack ParseProccessedDataTable(DataTable dataTable)
{
bool isVector = dataTable.Columns.Count > 7;
ProccessedTrack pTrack = new ProccessedTrack(dataTable.TableName.Replace('_', ' ').Replace("$", ""), isVector);
DataRow[] dataRows = dataTable.Select();
if (isVector)
{
foreach (DataRow row in dataRows)
{
double stlat = (double)row[1];
double stlon = (double)row[2];
DateTime stdate = (DateTime)row[3];
double enlat = (double)row[4];
double enlon = (double)row[5];
DateTime endate = (DateTime)row[6];
TimeSpan lapse = endate - stdate;
double oSpeed = (double)row[8];
double pSpeed = (double)row[9];
double oDist = (double)row[10];
double pDist = (double)row[11];
double oHead = (double)row[12];
double pHead = (double)row[13];
TrackItem stItem = new TrackItem(stlat, stlon, stdate);
TrackItem enItem = new TrackItem(enlat, enlon, endate);
TrackVectorItem vItem = new TrackVectorItem(stItem, enItem);
ProccessedTrackVectorItem pVector = new ProccessedTrackVectorItem(vItem, pSpeed, pDist, pHead);
pTrack.AddProccedTrackVectorItem(pVector);
}
}
else
{
foreach (DataRow row in dataRows)
{
double lat = (double)row[1];
double lon = (double)row[2];
DateTime date = (DateTime)row[3];
double plat = (double)row[4];
double plon = (double)row[5];
double dist = (double)row[6];
TrackItem item = new TrackItem(lat, lon, date);
ProccessedTrackItem pItem = new ProccessedTrackItem(item, plat, plon);
pTrack.AddProccessedTrackItem(pItem);
}
}
return pTrack;
}
/// <summary>
/// Получить трек со средними разносятми
/// </summary>
/// <param name="pTrack">Трек с разносятми</param>
/// <param name="track">Исходный трек</param>
/// <returns>Трек со средними разносятми</returns>
private ProccessedTrack GetAvgDifferences(ProccessedTrack pTrack, Track track)
{
ProccessedTrackItem[] pItems = pTrack.ProccessedTrackItems;
ProccessedTrack res = new ProccessedTrack("Средние разности");
for (int i = 0; i < avg / 2 + 1;i++ )
{
res.AddProccessedTrackItem(pItems[i]);
}
TrackItem[] trackItems = track.TrackItems;
for (int i = avg / 2 + 1;i<pItems.Length-avg/2 ; i++)
{
double sumLat = 0;
double sumLon = 0;
for (int j = i - avg / 2; j <= i + avg / 2; j++)
{
sumLat += pItems[j].ProccessedLatitude;
sumLon += pItems[j].ProccessedLongitude;
}
sumLat /= avg;
sumLon /= avg;
ProccessedTrackItem pItem = new ProccessedTrackItem(trackItems[i], sumLat, sumLon);
res.AddProccessedTrackItem(pItem);
}
for (int i = trackItems.Length - avg / 2; i < trackItems.Length; i++)
{
res.AddProccessedTrackItem(pItems[i]);
}
return res;
}
