public static double[] ApplyUKF(double[] Data)
{
double[] NewData = new double[Data.Length];
UKF Filter = new UKF();
for (int i = 0; i < Data.Length; i++)
{
Filter.Update(new[] { Data[i] });
NewData[i] = Filter.getState()[0];
}
return(NewData);
}
public void Calculate()
{
// Find max and min RPM index
int MaxRPMIdx = -1;
int MinRPMIdx = -1;
double MaxRPM = -1;
double MinRPM = -1;
Calculator.Weight2 = Weight;
for (int i = 0; i < DataEntries.Length; i++)
{
if (DataEntries[i][RPM] > MaxRPM)
{
MaxRPMIdx = i;
MaxRPM = DataEntries[i][RPM];
}
}
for (int i = MaxRPMIdx - 1; i >= 0; i--)
{
if (DataEntries[i][RPM] < DataEntries[i + 1][RPM])
{
MinRPMIdx = i;
MinRPM = DataEntries[i][RPM];
continue;
}
break;
}
if (MaxRPMIdx == -1 || MinRPMIdx == -1 || (MaxRPM - MinRPMIdx) <= 1)
{
throw new Exception("Invalid data");
}
// Trim data
DataEntries = DataEntries.Skip(MinRPMIdx).Take(MaxRPMIdx - MinRPMIdx).ToArray();
// Normalize time
double StartTime = DataEntries[0][DeviceTime];
for (int i = 0; i < DataEntries.Length; i++)
{
DataEntries[i][DeviceTime] = DataEntries[i][DeviceTime] - StartTime;
}
// Smooth out speed
/*double[] Speeds = DataEntries.Select(E => GetSpeed(E)).ToArray();
* Speeds = Utils.ApplyUKF(Speeds);
* for (int i = 0; i < DataEntries.Length; i++)
* SetSpeed(DataEntries[i], Speeds[i]);*/
// Calculate all data
const int CalcOffset = 1;
double Dt = 0;
DataEntries[0].Calculated = new CalculatedEntry();
UKF Filter = new UKF(DataEntries.Length);
double MaxPower = 0;
double MaxTorque = 0;
for (int i = CalcOffset; i < DataEntries.Length - CalcOffset; i++)
{
int PrevIdx = i - CalcOffset;
int NextIdx = i + CalcOffset;
double PrevTime = DataEntries[PrevIdx][DeviceTime];
double CurTime = DataEntries[i][DeviceTime];
double NextTime = DataEntries[NextIdx][DeviceTime];
Dt = Utils.Distance(PrevTime, NextTime);
double PrevSpeed = GetSpeed(PrevIdx);
double CurSpeed = GetSpeed(i);
double NextSpeed = GetSpeed(NextIdx);
double PowerRaw = Calculator.Calculate(NextSpeed, PrevSpeed, NextTime, PrevTime);
Filter.Update(new double[] { PowerRaw });
//double Power = Filter.getState()[0];
double Power = PowerRaw;
double Torque = Calculator.CalcTorque(Power * 1.2, DataEntries[i][RPM]);
if (Power > MaxPower)
{
MaxPower = Power;
}
if (Torque > MaxTorque)
{
MaxTorque = Torque;
}
DataEntries[i].Calculated = new CalculatedEntry(Power, PowerRaw, Torque);
}
Console.WriteLine("{0} whp; {1} Nm", MaxPower, MaxTorque);
// Fill last index
int LastIdx = DataEntries.Length - 1;
if (DataEntries[LastIdx].Calculated == null)
{
int Prev = FindPrevious(LastIdx, null);
DataEntries[LastIdx].Calculated = new CalculatedEntry(DataEntries[Prev].Calculated);
}
// Interpolate calculated data
for (int i = 0; i < DataEntries.Length; i++)
{
if (DataEntries[i].Calculated == null)
{
LogEntry Prev = DataEntries[FindPrevious(i, null)];
LogEntry Next = DataEntries[FindNext(i, null)];
DataEntries[i].Calculated = new CalculatedEntry(this, Prev, Next, DataEntries[i][DeviceTime]);
}
}
// Smooth out power raw
double[] PowerRaws = DataEntries.Select(E => E.Calculated.PowerRaw).ToArray();
Utils.NoiseReduction(ref PowerRaws, 1); // 4
for (int i = 0; i < PowerRaws.Length; i++)
{
DataEntries[i].Calculated.Power = PowerRaws[i];
}
}