Exemplo n.º 1
0
        public IMU(int dt)
        {
            accel = new Accelerometer();
            mag = new Magnometer();
            gyro = new Gyroscope();
            g = new GPS();

            K = new Kalman(dt);
        }
Exemplo n.º 2
0
        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());
        }