def test_measurementUpdateAHRS(self):
        """ Test the AHRS measurement update against MATLAB data """
        filename = "./test_data/measurementUpdateAHRSTest.mat"
        data = sio.loadmat(filename, struct_as_record=True)
        test_filter = ekf(np.squeeze(data['x_hat_i']), data['P_i'], data['Q'],
                          data['R_gps'], data['R_imu'])

        test_filter.timeUpdate(np.squeeze(data['u']), np.squeeze(data['time']))

        for x in range(data['numCases']):
            inp = data['y_imu'][x]
            (inv, S, K) = test_filter.measurementUpdateAHRS(inp)

            assert_array_almost_equal(inv,
                                      np.squeeze(data['innovation'][x]),
                                      err_msg="innvation, on iteration: " +
                                      str(x + 1))
            assert_array_almost_equal(S,data['S'][x], \
                    err_msg = "error covariance, on iteration: " + str(x+1))
            assert_array_almost_equal(K,data['K'][x], \
                    err_msg = "Kalman Gain, on iteration: " + str(x+1))
            assert_array_almost_equal(test_filter.x_hat,data['x_hat'][x], \
                    err_msg = "x_hat, on iteration: " + str(x+1))
            assert_array_almost_equal(test_filter.P,data['P'][x], \
                    err_msg = "P, on iteration: " + str(x+1))
    def test_measurementUpdateAHRS(self):
        """ Test the AHRS measurement update against MATLAB data """
        filename = "./test_data/measurementUpdateAHRSTest.mat"
        data = sio.loadmat(filename,struct_as_record=True)
        test_filter = ekf(
            np.squeeze(data['x_hat_i']),
            data['P_i'],
            data['Q'],
            data['R_gps'],
            data['R_imu'])

        test_filter.timeUpdate(np.squeeze(data['u']), np.squeeze(data['time']))

        for x in range(data['numCases']):
            inp = data['y_imu'][x]
            (inv, S, K) = test_filter.measurementUpdateAHRS(inp)
            
            assert_array_almost_equal(inv,np.squeeze(data['innovation'][x]),
                    err_msg = "innvation, on iteration: " + str(x+1))
            assert_array_almost_equal(S,data['S'][x], \
                    err_msg = "error covariance, on iteration: " + str(x+1))
            assert_array_almost_equal(K,data['K'][x], \
                    err_msg = "Kalman Gain, on iteration: " + str(x+1))
            assert_array_almost_equal(test_filter.x_hat,data['x_hat'][x], \
                    err_msg = "x_hat, on iteration: " + str(x+1))
            assert_array_almost_equal(test_filter.P,data['P'][x], \
                    err_msg = "P, on iteration: " + str(x+1))
 def test_modelUpdate(self):
     """ Test the model update against the equivalent MATLAB function """
     for x in range(1, 6):
         filename = "./test_data/UpdateModelTest" + str(x) + ".mat"
         data = sio.loadmat(filename, struct_as_record=True)
         test_filter = ekf(np.squeeze(data['x_hat_i']), data['P'],
                           data['Q'], data['R_gps'], data['R_imu'])
         test_filter.updateModel(np.squeeze(data['u']),
                                 np.squeeze(data['dt']))
         assert_array_equal(data['F'], test_filter.F)
         assert_array_equal(data['G'], test_filter.G)
 def test_modelUpdate(self):
     """ Test the model update against the equivalent MATLAB function """
     for x in range(1,6):
         filename = "./test_data/UpdateModelTest" + str(x) + ".mat" 
         data = sio.loadmat(filename,struct_as_record=True)
         test_filter = ekf(
             np.squeeze(data['x_hat_i']),
             data['P'],
             data['Q'],
             data['R_gps'],
             data['R_imu'])
         test_filter.updateModel(np.squeeze(data['u']),np.squeeze(data['dt']))
         assert_array_equal(data['F'],test_filter.F)
         assert_array_equal(data['G'],test_filter.G)
 def test_TimeUpdate(self):
     """ Test the time update against MATLAB data """
     filename = "./test_data/TimeUpdateTest.mat"
     data = sio.loadmat(filename, struct_as_record=True)
     test_filter = ekf(np.squeeze(data['x_hat_i']), data['P_i'], data['Q'],
                       data['R_gps'], data['R_imu'])
     for x in range(len(data['u'])):
         (v,w) = test_filter.timeUpdate(np.squeeze(data['u'][x]), \
                 np.squeeze(data['time'][x]))
         assert_almost_equal(v,np.squeeze(data['v'][0,x]),decimal=10, \
                 err_msg = "v, On Iteration: " + str(x+1))
         assert_almost_equal(w,np.squeeze(data['w'][0,x]),decimal=10, \
                 err_msg = "w, On Iteration: " + str(x+1))
         assert_array_almost_equal(test_filter.x_hat, \
                 np.squeeze(data['x_hat'][x]),decimal=10, \
                 err_msg = "x_hat, On Iteration: " + str(x+1))
         assert_array_almost_equal(data['P'][x],test_filter.P, \
                 decimal=10, err_msg = "P, On Iteration: " + str(x+1))
 def test_TimeUpdate(self):
     """ Test the time update against MATLAB data """
     filename = "./test_data/TimeUpdateTest.mat"
     data = sio.loadmat(filename,struct_as_record=True)
     test_filter = ekf(
         np.squeeze(data['x_hat_i']),
         data['P_i'],
         data['Q'],
         data['R_gps'],
         data['R_imu'])
     for x in range(len(data['u'])):
         (v,w) = test_filter.timeUpdate(np.squeeze(data['u'][x]), \
                 np.squeeze(data['time'][x]))
         assert_almost_equal(v,np.squeeze(data['v'][0,x]),decimal=10, \
                 err_msg = "v, On Iteration: " + str(x+1))
         assert_almost_equal(w,np.squeeze(data['w'][0,x]),decimal=10, \
                 err_msg = "w, On Iteration: " + str(x+1))
         assert_array_almost_equal(test_filter.x_hat, \
                 np.squeeze(data['x_hat'][x]),decimal=10, \
                 err_msg = "x_hat, On Iteration: " + str(x+1))
         assert_array_almost_equal(data['P'][x],test_filter.P, \
                 decimal=10, err_msg = "P, On Iteration: " + str(x+1))