public IMU(int dt)
{
accel = new Accelerometer();
mag = new Magnometer();
gyro = new Gyroscope();
g = new GPS();
K = new Kalman(dt);
}
public void Update(Magnometer m, Gyroscope gyro, Accelerometer a, GPS g)
{
Measurement[0] = g.Lat;
Measurement[1] = a.X;
Measurement[2] = g.Long;
Measurement[3] = a.Y;
Measurement[4] = m.Heading;
Measurement[5] = gyro.Z;
//should run on first itteration
if (state == null)
state = Measurement;
//Predict new states
stateP = Multiply(A, state);
//Predict Process Covariance MAtrix
//P_future = A*P*A^t (^t is transpose);
P_future = Multiply(Multiply(A,P),Transpose(A));
//Update new measurements
//K = (P_future*H^t)/(H*P_future*H^t + R); //element wise division
//since H is the idenity matrix, to save time no multiplication needs to be done
K = Divide(P_future, (Add(P_future,R)));
state = Add(stateP, Multiply(K, Subtract(Measurement, stateP)));
//Output
P = Multiply(Subtract(H,K), P_future);
//state;
//write data to file
//var csv = new StringBuilder();
//var first = Measurement[0];
//var second = Measurement[1];
//var third = Measurement[2];
//var fourth = Measurement[3];
//var fifth = Measurement[4];
//var sixth = Measurement[5];
//var newLine = string.Format("{0},{1},{2},{3},{4},{5}{6}", first, second, third, fourth, fifth, sixth, Environment.NewLine);
//csv.Append(newLine);
//compare output to Data.csv
//System.IO.File.AppendAllText("Data1.csv", csv.ToString());
}