def __init__(self, static_properties): self.static_properties = static_properties self.lat_ref = self.static_properties.initial_lat self.long_ref = self.static_properties.initial_long self.alt_ref = self.static_properties.initial_alt # Outputs of the Kalman filter # in NED, starting point is relative self.estimated_position = np.matrix(navpy.lla2ecef(self.lat_ref, self.long_ref, self.alt_ref)).T # prev gps variables self.prev_gps_position_ecef = np.matrix(navpy.lla2ecef(self.lat_ref, self.long_ref, self.alt_ref)).T self.estimated_velocity = self.ned_to_ecef( self.static_properties.initial_v_north, self.static_properties.initial_v_east, self.static_properties.initial_v_down, ) # initial velocity should be given in meters with respect to your NED # old_est_C_b_e in Loosely_coupled_INS_GNSS (line 166) self.estimated_attitude = navpy.angle2dcm( self.static_properties.initial_yaw, self.static_properties.initial_pitch, self.static_properties.initial_roll, )
def get_gps_orientation(lat1, long1, alt1, lat2, long2, alt2): pos1 = navpy.lla2ecef(lat1, long1, alt1) pos2 = navpy.lla2ecef(lat2, long2, alt2) delta_pos = pos2 - pos1 yaw = math.atan2(delta_pos[1], delta_pos[0]) pitch = math.atan2(delta_pos[2] * math.cos(yaw), delta_pos[0]) roll = math.atan2(math.cos(yaw), math.sin(yaw) * math.sin(pitch)) return yaw, pitch, roll
def test_lla2ecef_vector(self): """ Test conversion of multiple LLA to ECEF Data Source: Pretoria and Sydney, Exercise 2.4 and 2.5 of Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel 2008 """ ecef_pretoria = [5.057590377e6, 2.694861463e6, -2.794229000e6]#in meters lat_pretoria = -(26. + 8./60 + 42.20/3600) # South lon_pretoria = +(28. + 3./60 + 0.92/3600) # East alt_pretoria = 1660.86 # [meters] ecef_sydney = [-4.646678571e6, 2.549341033e6, -3.536478881e6] #in meters lat_sydney = -( 33. + 53./60 + 28.15/3600) # South lon_sydney = +(151. + 14./60 + 57.07/3600) # East alt_sydney = 86.26 # [meters] lat = np.array([lat_pretoria,lat_sydney]) lon = [lon_pretoria,lon_sydney] alt = [alt_pretoria,alt_sydney] ecef = np.vstack((ecef_pretoria,ecef_sydney)) # Do conversion and check result ecef_computed = navpy.lla2ecef(lat,lon,alt) # Note: see comment in test_lla2ecef_Ausralia() as to why # only 2 digits of accuracy is being tested for the example. np.testing.assert_almost_equal(ecef_computed,ecef,decimal=2)
def test_helpers(): lat1, long1, alt1 = 40.4404285, -79.9422232, 296.18 ecef_position = navpy.lla2ecef(lat1, long1, alt1) x, y, z = ecef_position almost_equal(x, 848993, epsilon=0.5) almost_equal(y, -4786645, epsilon=0.5) almost_equal(z, 4115520, epsilon=0.5) test_coordinate_transforms(40.44057846069336, 79.94245147705078, 302.79998779296875)
def test_coordinate_transforms(lat1, long1, alt1): ecef_position = navpy.lla2ecef(lat1, long1, alt1) # print(ecef_position) lat2, long2, alt2 = navpy.ecef2lla(ecef_position) # print(lat2, long2, alt2) almost_equal(lat1, lat2) almost_equal(long1, long2) almost_equal(alt1, alt2)
def test_lla2ecef(self): """ Test conversion of LLA to ECEF. Data Source: Example 2.1 of Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel 2008 """ # Near Los Angeles, CA lat = +(34. + 0./60 + 0.00174/3600) # North lon = -(117. + 20./60 + 0.84965/3600) # West alt = 251.702 # [meters] ecef = [-2430601.828, -4702442.703, 3546587.358] # [meters] # Do conversion and check result # Note: default units are degrees ecef_computed = navpy.lla2ecef(lat, lon, alt) for e1, e2 in zip(ecef_computed, ecef): self.assertAlmostEqual(e1, e2, places=3)
def test_lla2ecef_SAfrica(self): """ Test conversion of LLA to ECEF. Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel 2008 Note: N, E (+) and S, W (-) """ # Pretoria S. Africa lat = -(26. + 8./60 + 42.20/3600) # South lon = +(28. + 3./60 + 0.92/3600) # East alt = 1660.86 # [meters] ecef = [5.057590377e6, 2.694861463e6, -2.794229000e6] # [meters] # Do conversion and check result # Note: default units are degrees ecef_computed = navpy.lla2ecef(lat, lon, alt) # Note: see comment in test_lla2ecef_Ausralia() as to why # only 2 digits of accuracy is being tested for the example. for e1, e2 in zip(ecef_computed, ecef): self.assertAlmostEqual(e1, e2, places=2)
def test_lla2ecef_Ausralia(self): """ Test conversion of LLA to ECEF. Data Source: Exercise 2.4 and 2.5 of Aided Navigation: GPS with High Rate Sensors, Jay A. Farrel 2008 Note: N, E (+) and S, W (-) """ # Sydney Australia lat = -( 33. + 53./60 + 28.15/3600) # South lon = +(151. + 14./60 + 57.07/3600) # East alt = 86.26 # [meters] # Note: The book's example only seems reliable to 2 digits passed # decimal. For example, ecef_x is recorded as xxx.571, but # even using the books equation (2.7 and 2.9) you get xxx.572555 ecef = [-4.646678571e6, 2.549341033e6, -3.536478881e6] # [meters] # Do conversion and check result # Note: default units are deg ecef_computed = navpy.lla2ecef(lat, lon, alt) for e1, e2 in zip(ecef_computed, ecef): self.assertAlmostEqual(e1, e2, places=2)
def run_filter(filter, imu_data, gps_data, filter_data, config=None): data_dict = data_store.data_store() errors = [] # Using while loop starting at k (set to kstart) and going to end # of .mat file gps_index = 0 filter_index = 0 new_gps = 0 if config and config['call_init']: filter_init = False else: filter_init = True if config and 'start_time' in config: for k, imu_pt in enumerate(imu_data): if imu_pt.time >= config['start_time']: k_start = k break else: k_start = 0 if config and 'end_time' in config: for k, imu_pt in enumerate(imu_data): if imu_pt.time >= config['end_time']: k_end = k break else: k_end = len(imu_data) #print k_start, k_end for k in range(k_start, k_end): imupt = imu_data[k] if gps_index < len(gps_data) - 1: # walk the gps counter forward as needed newData = 0 while gps_data[gps_index+1].time - gps_latency <= imupt.time: gps_index += 1 newData = 1 gpspt = gps_data[gps_index] gpspt.newData = newData else: # no more gps data, stay on the last record gpspt = gps_data[gps_index] gpspt.newData = 0 #print gpspt.time` # walk the filter counter forward as needed if len(filter_data): if imupt.time > filter_data[filter_index].time: filter_index += 1 if filter_index >= len(filter_data): # no more filter data, stay on the last record filter_index = len(filter_data)-1 filterpt = filter_data[filter_index] else: filterpt = None #print "t(imu) = " + str(imupt.time) + " t(gps) = " + str(gpspt.time) # If k is at the initialization time init_nav else get_nav if not filter_init and gps_index > 0: print "init:", imupt.time, gpspt.time navpt = filter.init(imupt, gpspt, filterpt) filter_init = True elif filter_init: navpt = filter.update(imupt, gpspt, filterpt) # Store the desired results obtained from the compiled test # navigation filter and the baseline filter if filter_init: data_dict.append(navpt) if gpspt.newData: # compute error metric with each new gps report # full 3d distance error (in ecef) p1 = navpy.lla2ecef(gpspt.lat, gpspt.lon, gpspt.alt, latlon_unit='deg') p2 = navpy.lla2ecef(navpt.lat, navpt.lon, navpt.alt, latlon_unit='rad') pe = np.linalg.norm(p1 - p2) # weight horizontal error more highly than vertical error ref = gps_data[0] n1 = navpy.lla2ned(gpspt.lat, gpspt.lon, gpspt.alt, ref.lat, ref.lon, 0.0, latlon_unit='deg') n2 = navpy.lla2ned(navpt.lat*r2d, navpt.lon*r2d, navpt.alt, ref.lat, ref.lon, 0.0, latlon_unit='deg') dn = n2 - n1 ne = math.sqrt(dn[0]*dn[0] + dn[1]*dn[1] + dn[2]*dn[2]*0.5) # it is always tempting to fit to the velocity vector # (especially when seeing some of the weird velocity fits # that the optimizer spews out), but it never helps # ... seems to make the solution convergence much more # shallow, ultimately never seems to produce a better fit # than using position directly. Weird fits happen when # the inertial errors just don't fit the gps errors. # Fitting to velocity doesn't seem to improve that # problem. v1 = np.array( [gpspt.vn, gpspt.ve, gpspt.vd] ) v2 = np.array( [navpt.vn, navpt.ve, navpt.vd] ) ve = np.linalg.norm(v1 - v2) errors.append(ne) # ned error weighted towards horizontal # Increment time up one step for the next iteration of the # while loop. k += 1 # proper cleanup filter.close() return errors, data_dict
def test_angle_transform(): g = 9.80665 static_properties = StaticProperties( initial_lat=40.44057846069336, initial_long=79.94245147705078, initial_alt=302.79998779296875, initial_roll=0, initial_pitch=0, initial_yaw=0, roll_error=math.radians(-0.05), pitch_error=math.radians(0.04), yaw_error=math.radians(1), initial_v_north=0, initial_v_east=0, initial_v_down=0, initial_attitude_unc=math.radians(1), initial_velocity_unc=0.1, initial_position_unc=10, initial_accel_bias_unc=(g * 1e-3) ** 2, initial_gyro_bias_unc=math.radians(1 / 3600), initial_clock_offset_unc=10000, initial_clock_drift_unc=100, gyro_noise_PSD=math.radians(0.02 / 60) ** 2, accel_noise_PSD=200 * (g * 1e-6) ** 2, accel_bias_PSD=1.0e-7, gyro_bias_PSD=2.0e-12, pos_meas_SD=2.5, vel_meas_SD=0.1, ) frame_properties = CoordinateFrame(static_properties) ins = INS(frame_properties) gyro_measurement = np.matrix([0, 0, math.pi / 2]).T accel_measurement = np.matrix([1, 0, 0]).T dt = 0.001 earth_rotation_amount, earth_rotation_matrix = ins.get_earth_rotation_matrix(dt) mag_angle_change, skew_gyro_body = ins.get_gyro_skew_matrix(dt, gyro_measurement) estimated_attitude = ins.get_estimated_attitude( mag_angle_change, skew_gyro_body, earth_rotation_matrix, frame_properties.estimated_attitude ) average_attitude = ins.get_average_attitude( mag_angle_change, estimated_attitude, skew_gyro_body, earth_rotation_amount ) accel_meas_ecef = average_attitude * accel_measurement print("accel_meas_ecef") print(accel_meas_ecef, linalg.norm(accel_meas_ecef)) print("estimated_attitude") print(estimated_attitude, linalg.norm(estimated_attitude)) print("average_attitude") print(average_attitude, linalg.norm(average_attitude)) print("mag_angle_change") print(mag_angle_change) prev_est_p = frame_properties.estimated_position prev_est_v = frame_properties.estimated_velocity estimated_velocity = prev_est_v + dt * ( accel_meas_ecef - 2 * skew_symmetric(np.matrix([0, 0, earth_rotation_rate]).T) * prev_est_v ) # update cartesian position estimated_position = prev_est_p + (estimated_velocity + prev_est_v) * 0.5 * dt delta_position = ( estimated_position - np.matrix( navpy.lla2ecef( static_properties.initial_lat, static_properties.initial_long, static_properties.initial_alt ) ).T ) print("estimated_position") print(delta_position, linalg.norm(delta_position)) print("estimated_velocity") print(estimated_velocity, linalg.norm(estimated_velocity))
# Calculate Attitude Error delta_att = np.nan*np.zeros((drl,3)) for i in range(0,drl): # when processing real data, we have no truth reference #C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T C1 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T C2 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T dC = C2.dot(C1.T)-np.eye(3) # dC contains delta angle. To match delta quaternion, divide by 2. delta_att[i,:] = [-dC[1,2]/2.0, dC[0,2]/2.0, -dC[0,1]/2.0] lat_ref = estPOS[idx_init[0],0] lon_ref = estPOS[idx_init[0],1] alt_ref = estPOS[idx_init[0],2] ecef_ref = navpy.lla2ecef(lat_ref,lon_ref,alt_ref) ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) filter_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) # when processing real data, we have no truth reference #ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt) #ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) #gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) #gnss_ned[0:idx_init[0],:] = np.nan rawgps = np.nan*np.ones((len(gps_data),3)) for i, gps in enumerate(gps_data): rawgps[i,:] = [ gps.lat, gps.lon, gps.alt ] ecef = navpy.lla2ecef(rawgps[:,0],rawgps[:,1],rawgps[:,2],latlon_unit='deg') ref_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
def lla_to_ecef(self, lat, long, alt): return np.matrix(navpy.lla2ecef(lat, long, alt)).T
def __init__(self): self.truthRos = Odometry() self.imuRos = Imu() self.relPosRos = RelPos() self.baseGpsRos = PosVelEcef() self.roverGpsRos = PosVelEcef() self.rtkCompassRos = RelPos() self.truth = TruthMsg() self.roverTruth = TruthMsg() self.imu = ImuMsg() self.base2RoverRelPos = RelPosMsg() self.baseGps = GpsMsg() self.roverGps = GpsMsg() self.rtkCompass = GpsCompassMsg self.accelerometerAccuracyStdDev = 0.025 self.gyroAccuracyStdDev = 0.0023 self.gpsHorizontalAccuracyStdDev = 0.2 self.gpsVerticalAccuracyStdDev = 0.4 self.gpsSpeedAccuracyStdDev = 0.2 self.rtkHorizontalAccuracyStdDev = 0.02 self.rtkVerticalAccuracyStdDev = 0.04 self.rtkCompassAccuracyDegStdDev = 1.0 #depends on baseline. This also uses RTK #These are to remember noise of previous update. They are used in the low pass filter self.gpsNoise = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.rtkCompassNoise = 0.0 self.accelerometerBias = [0.3, -0.5, -0.2] self.gyroBias = [0.01, 0.08, -0.02] self.imuTs = 1.0 / 200.0 self.gpsTs = 1.0 / 5.0 self.firstCallback = True self.firstTime = 0.0 self.latRef = 20.24 self.lonRef = -111.66 self.altRef = 1387.0 self.originEcef = navpy.lla2ecef(self.latRef, self.lonRef, self.altRef) self.gravity = np.array([[0.0, 0.0, 9.81]]).T self.lowPassFilterAlpha = 0.9 self.truth_pub_ = rospy.Publisher('base_truth', Odometry, queue_size=5, latch=True) self.imu_pub_ = rospy.Publisher('base_imu', Imu, queue_size=5, latch=True) self.relPos_pub_ = rospy.Publisher('base2Rover_relPos', RelPos, queue_size=5, latch=True) self.base_gps_pub_ = rospy.Publisher('base_gps', PosVelEcef, queue_size=5, latch=True) self.rover_gps_pub_ = rospy.Publisher('rover_gps', PosVelEcef, queue_size=5, latch=True) self.rtk_compass_pub_ = rospy.Publisher('base_rtk_compass', RelPos, queue_size=5, latch=True) self.truth_rate_timer_ = rospy.Timer(rospy.Duration(self.imuTs), self.truthCallback) self.gps_rate_timer_ = rospy.Timer(rospy.Duration(self.gpsTs), self.gpsCallback) while not rospy.is_shutdown(): rospy.spin()
def run_filter(filter, imu_data, gps_data, filter_data, config=None): data_dict = data_store.data_store() errors = [] # Using while loop starting at k (set to kstart) and going to end # of .mat file gps_index = 0 filter_index = 0 new_gps = 0 if config and config['call_init']: filter_init = False else: filter_init = True if config and 'start_time' in config: for k, imu_pt in enumerate(imu_data): if imu_pt.time >= config['start_time']: k_start = k break else: k_start = 0 if config and 'end_time' in config: for k, imu_pt in enumerate(imu_data): if imu_pt.time >= config['end_time']: k_end = k break else: k_end = len(imu_data) #print k_start, k_end for k in range(k_start, k_end): imupt = imu_data[k] if gps_index < len(gps_data) - 1: # walk the gps counter forward as needed newData = 0 while gps_data[gps_index+1].time - gps_latency <= imupt.time: gps_index += 1 newData = 1 gpspt = gps_data[gps_index] gpspt.newData = newData else: # no more gps data, stay on the last record gpspt = gps_data[gps_index] gpspt.newData = 0 #print gpspt.time` # walk the filter counter forward as needed if len(filter_data): if imupt.time > filter_data[filter_index].time: filter_index += 1 if filter_index >= len(filter_data): # no more filter data, stay on the last record filter_index = len(filter_data)-1 filterpt = filter_data[filter_index] else: filterpt = None #print "t(imu) = " + str(imupt.time) + " t(gps) = " + str(gpspt.time) # If k is at the initialization time init_nav else get_nav if not filter_init and gps_index > 0: print "init:", imupt.time, gpspt.time navpt = filter.init(imupt, gpspt, filterpt) filter_init = True elif filter_init: navpt = filter.update(imupt, gpspt, filterpt) # Store the desired results obtained from the compiled test # navigation filter and the baseline filter if filter_init: data_dict.append(navpt, imupt) if gpspt.newData: # compute error metric with each new gps report # full 3d distance error (in ecef) p1 = navpy.lla2ecef(gpspt.lat, gpspt.lon, gpspt.alt, latlon_unit='deg') p2 = navpy.lla2ecef(navpt.lat, navpt.lon, navpt.alt, latlon_unit='rad') pe = np.linalg.norm(p1 - p2) # weight horizontal error more highly than vertical error ref = gps_data[0] n1 = navpy.lla2ned(gpspt.lat, gpspt.lon, gpspt.alt, ref.lat, ref.lon, 0.0, latlon_unit='deg') n2 = navpy.lla2ned(navpt.lat*r2d, navpt.lon*r2d, navpt.alt, ref.lat, ref.lon, 0.0, latlon_unit='deg') dn = n2 - n1 ne = math.sqrt(dn[0]*dn[0] + dn[1]*dn[1] + dn[2]*dn[2]*0.5) # it is always tempting to fit to the velocity vector # (especially when seeing some of the weird velocity fits # that the optimizer spews out), but it never helps # ... seems to make the solution convergence much more # shallow, ultimately never seems to produce a better fit # than using position directly. Weird fits happen when # the inertial errors just don't fit the gps errors. # Fitting to velocity doesn't seem to improve that # problem. v1 = np.array( [gpspt.vn, gpspt.ve, gpspt.vd] ) v2 = np.array( [navpt.vn, navpt.ve, navpt.vd] ) ve = np.linalg.norm(v1 - v2) errors.append(ne) # ned error weighted towards horizontal # Increment time up one step for the next iteration of the # while loop. k += 1 # proper cleanup filter.close() return errors, data_dict
def update(self, imu, gps, verbose=True): # Check for validity of GNSS at this time self.tcpu = imu.time self.tow = gps.tow # Test 1: navValid flag and the data is indeed new (new Time of Week) # Execute Measurement Update if this is true and Test 2 passes NEW_GNSS_FLAG = ((gps.valid == 0) & (abs(self.tow - self.last_tow) > 1e-3)) # Test 2: Check if the delta time of the Time of Week and the # CPU time are consistent # If this fails, re-initialize the filter. There must have been a glitch if NEW_GNSS_FLAG: #print self.tcpu, self.last_tcpu, self.tcpu-self.last_tcpu #print self.tow, self.last_tow, self.tow-self.last_tow if abs((self.tcpu - self.last_tcpu) - (self.tow - self.last_tow)) > 0.5: self.TU_COUNT = 0 self.NAV_INIT = False if verbose: print("Time Sync Error -- Request reinitialization") # Record the tow and tcpu at this new update self.last_tow = self.tow self.last_tcpu = self.tcpu # Different subroutine executed in the filter if not self.NAV_INIT: if not self.IMU_CAL_INIT: # SUBROUTINE: IMU CALIBRATION, # This only happens first time round on the ground. Inflight # reinitialization is not the same. self.estAB[:] = [0, 0, 0] if self.very_first_time: self.very_first_time = False self.estGB[:] = [imu.p, imu.q, imu.r] self.phi = 0 self.theta = 0 else: self.estGB[:] = self.last_estGB[:]*self.TU_COUNT \ + [imu.p, imu.q, imu.r] # Simple AHRS values self.phi = self.phi*self.TU_COUNT \ + np.arctan2(-imu.ay, -imu.az) self.theta = self.theta*self.TU_COUNT \ + np.arctan2(imu.ax, np.sqrt(imu.ay**2+imu.az**2)) self.phi /= (self.TU_COUNT + 1) self.theta /= (self.TU_COUNT + 1) self.estGB[:] /= (self.TU_COUNT + 1) self.estATT[0], self.estATT[1:] = navpy.angle2quat( 0, self.theta, self.phi) """ print("t = %7.3f, Gyro Bias Value: [%6.2f, %6.2f, %6.2f] deg/sec" %\ (imu.time, np.rad2deg(self.estGB[0]), np.rad2deg(self.estGB[1]), np.rad2deg(self.estGB[2]) )) print("t = %7.3f, phi = %6.2f, theta = %6.2f" % (imu.time,np.rad2deg(self.phi),np.rad2deg(self.theta)) ) """ self.TU_COUNT += 1 if self.TU_COUNT >= 35: self.TU_COUNT = 0 self.IMU_CAL_INIT = True if verbose: print("t = %7.3f, IMU Calibrated!" % (imu.time)) del (self.phi) del (self.theta) else: if not NEW_GNSS_FLAG: # SUBROUTINE 1: BACKUP NAVIGATION or IN-FLIGHT INITIALIZATION # >>>> AHRS CODE GOES HERE self.estATT[:] = self.last_estATT[:] # This should be some backup nav mode # <<<< # When still there is no GNSS signal, continue propagating bias self.estAB[:] = self.last_estAB[:] self.estGB[:] = self.last_estGB[:] else: # When there is GNSS fix available, initialize all the states # and renew covariance self.estPOS[:] = [gps.lat, gps.lon, gps.alt] self.estVEL[:] = [gps.vn, gps.ve, gps.vd] self.estATT[:] = self.last_estATT[:] #self.estATT[0], self.estATT[1:] = navpy.angle2quat(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]) self.estAB[:] = self.last_estAB[:] self.estGB[:] = self.last_estGB[:] self.init_covariance() #idx_init.append(i) self.NAV_INIT = True if verbose: print("t = %7.3f, Filter (Re-)initialized" % (imu.time)) elif self.NAV_INIT: # SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS # ==== Time-Update ==== dt = imu.time - self.last_imu.time q0, qvec = self.last_estATT[0], self.last_estATT[1:4] C_B2N = navpy.quat2dcm(q0, qvec).T # 0. Data Acquisition f_b=np.array([0.5*(self.last_imu.ax+imu.ax)-self.last_estAB[0],\ 0.5*(self.last_imu.ay+imu.ay)-self.last_estAB[1],\ 0.5*(self.last_imu.az+imu.az)-self.last_estAB[2]]) om_ib=np.array([0.5*(self.last_imu.p+imu.p)-self.last_estGB[0],\ 0.5*(self.last_imu.q+imu.q)-self.last_estGB[1],\ 0.5*(self.last_imu.r+imu.r)-self.last_estGB[2]]) # 1. Attitude Update # --> Need to compensate for navrate and earthrate dqvec = 0.5 * om_ib * dt self.estATT[0], self.estATT[1:4] = navpy.qmult( q0, qvec, 1.0, dqvec) self.estATT[0] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) # 2. Velocity Update # --> Need to compensate for coriolis effect g = np.array([0, 0, 9.81]) f0, fvec = navpy.qmult(q0, qvec, 0, f_b) f_n0, f_nvec = navpy.qmult(f0, fvec, q0, -qvec) self.estVEL[:] = self.last_estVEL[:] + (f_nvec + g) * dt # 3. Position Update dPOS = navpy.llarate(self.last_estVEL[0], self.last_estVEL[1], self.last_estVEL[2], self.last_estPOS[0], self.last_estPOS[2]) dPOS *= dt self.estPOS[:] = self.last_estPOS[:] + dPOS # 4. Biases are constant self.estAB[:] = self.last_estAB[:] self.estGB[:] = self.last_estGB[:] # 5. Jacobian pos2pos = np.zeros((3, 3)) pos2gs = np.eye(3) pos2att = np.zeros((3, 3)) pos2acc = np.zeros((3, 3)) pos2gyr = np.zeros((3, 3)) gs2pos = np.zeros((3, 3)) gs2pos[2, 2] = -2 * 9.81 / wgs84.R0 gs2gs = np.zeros((3, 3)) gs2att = -2 * C_B2N.dot(navpy.skew(f_b)) gs2acc = -C_B2N gs2gyr = np.zeros((3, 3)) att2pos = np.zeros((3, 3)) att2gs = np.zeros((3, 3)) att2att = -navpy.skew(om_ib) att2acc = np.zeros((3, 3)) att2gyr = -0.5 * np.eye(3) F = np.zeros((15, 15)) F[0:3, 0:3] = pos2pos F[0:3, 3:6] = pos2gs F[0:3, 6:9] = pos2att F[0:3, 9:12] = pos2acc F[0:3, 12:15] = pos2gyr F[3:6, 0:3] = gs2pos F[3:6, 3:6] = gs2gs F[3:6, 6:9] = gs2att F[3:6, 9:12] = gs2acc F[3:6, 12:15] = gs2gyr F[6:9, 0:3] = att2pos F[6:9, 3:6] = att2gs F[6:9, 6:9] = att2att F[6:9, 9:12] = att2acc F[6:9, 12:15] = att2gyr F[9:12, 9:12] = -1.0 / self.tau_a * np.eye(3) F[12:15, 12:15] = -1.0 / self.tau_g * np.eye(3) PHI = np.eye(15) + F * dt # 6. Process Noise G = np.zeros((15, 12)) G[3:6, 0:3] = -C_B2N G[6:9, 3:6] = att2gyr G[9:12, 6:9] = np.eye(3) G[12:15, 9:12] = np.eye(3) Q = G.dot(self.Rw.dot(G.T)) * dt # 7. Covariance Update self.P = PHI.dot(self.P.dot(PHI.T)) + Q self.TU_COUNT += 1 if self.TU_COUNT >= 500: # Request reinitialization after 12 seconds of no GNSS updates self.TU_COUNT = 0 self.NAV_INIT = False if NEW_GNSS_FLAG: # ==== Measurement-Update ==== # 0. Get measurement and make innovations ecef_ref = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], 0) ins_ecef = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], self.estPOS[2]) gnss_ecef = navpy.lla2ecef(gps.lat, gps.lon, gps.alt) ins_ned = navpy.ecef2ned(ins_ecef - ecef_ref, self.estPOS[0], self.estPOS[1], self.estPOS[2]) gnss_ned = navpy.ecef2ned(gnss_ecef - ecef_ref, self.estPOS[0], self.estPOS[1], self.estPOS[2]) dpos = gnss_ned - ins_ned gnss_vel = np.array([gps.vn, gps.ve, gps.vd]) dvel = gnss_vel - self.estVEL[:] dy = np.hstack((dpos, dvel)) self.stateInnov[:] = dy # 1. Kalman Gain K = self.P.dot(self.H.T) K = K.dot(la.inv(self.H.dot(self.P.dot(self.H.T)) + self.R)) # 2. Covariance Update ImKH = np.eye(15) - K.dot(self.H) KRKt = K.dot(self.R.dot(K.T)) self.P = ImKH.dot(self.P.dot(ImKH.T)) + KRKt # 3. State Update dx = K.dot(dy) Rew, Rns = navpy.earthrad(self.estPOS[0]) self.estPOS[2] -= dx[2] self.estPOS[0] += np.rad2deg(dx[0] / (Rew + self.estPOS[2])) self.estPOS[1] += np.rad2deg( dx[1] / (Rns + self.estPOS[2]) / np.cos(np.deg2rad(self.estPOS[0]))) self.estVEL[:] += dx[3:6] self.estATT[0], self.estATT[1:4] = navpy.qmult( self.estATT[0], self.estATT[1:4], 1, dx[6:9]) self.estATT[0] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) self.estAB[:] += dx[9:12] self.estGB[:] += dx[12:15] if verbose: print("t = %7.3f, GNSS Update, self.TU_COUNT = %d" %\ (gps.time, self.TU_COUNT) ) self.TU_COUNT = 0 self.last_estPOS[:] = self.estPOS[:] self.last_estVEL[:] = self.estVEL[:] self.last_estATT[:] = self.estATT[:] self.last_estAB[:] = self.estAB[:] self.last_estGB[:] = self.estGB[:] else: # SUBROUTINE 3: e.g. BACKUP NAVIGATION MODE pass self.last_imu = imu self.last_gps = gps result = INSGPS(self.NAV_INIT, imu.time, self.estPOS, self.estVEL, self.estATT, self.estAB, self.estGB, self.P, self.stateInnov) return result
print("Problem #1:") print("-----------") lat = 30 # deg lon = 90 # deg alt = 0 earth_gm = 3.986004419e14 # Part a ecef_sphere = nav.lla2ecef(lat, lon, alt, latlon_unit='deg', alt_unit='m', model='xxx') # Part b ecef_wgs84 = nav.lla2ecef(lat, lon, alt, latlon_unit='deg', alt_unit='m', model='wgs84') ecef_diff = ecef_wgs84 - ecef_sphere file = open('Problem1ab.csv', 'w') csvWriter = csv.writer(file, lineterminator='\n')
psi, theta, phi = navpy.quat2angle(estATT[:,0],estATT[:,1:4],output_unit='deg') # Calculate Attitude Error delta_att = np.nan*np.zeros((drl,3)) for i in range(0,drl): C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T C2 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T dC = C2.dot(C1.T)-np.eye(3) # dC contains delta angle. To match delta quaternion, divide by 2. delta_att[i,:] = [-dC[1,2]/2.0, dC[0,2]/2.0, -dC[0,1]/2.0] lat_ref = estPOS[idx_init[0],0] lon_ref = estPOS[idx_init[0],1] alt_ref = estPOS[idx_init[0],2] ecef_ref = navpy.lla2ecef(lat_ref,lon_ref,alt_ref) ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) filter_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt) gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) gnss_ned[0:idx_init[0],:] = np.nan ecef = navpy.lla2ecef(flight_data.navlat,flight_data.navlon,flight_data.navalt,latlon_unit='rad') ref_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) ref_ned[0:idx_init[0],:] = np.nan # ============================= INS PLOTS ====================================== nsig = 3 istart = idx_init[0]
# when processing real data, we have no truth reference #C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T C1 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T C2 = navpy.angle2dcm(psi[i],theta[i],phi[i],input_unit='deg').T dC = C2.dot(C1.T)-np.eye(3) # dC contains delta angle. To match delta quaternion, divide by 2. delta_att[i,:] = [-dC[1,2]/2.0, dC[0,2]/2.0, -dC[0,1]/2.0] print "idx_init[0]: ", idx_init[0] print "len(estPOS): ", len(estPOS) lat_ref = estPOS[idx_init[0],0] lon_ref = estPOS[idx_init[0],1] alt_ref = estPOS[idx_init[0],2] ecef_ref = navpy.lla2ecef(lat_ref,lon_ref,alt_ref) ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) filter_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) # when processing real data, we have no truth reference #ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt) #ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) #gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) #gnss_ned[0:idx_init[0],:] = np.nan rawgps = np.nan*np.ones((len(gps_data),3)) for i, gps in enumerate(gps_data): rawgps[i,:] = [ gps.lat, gps.lon, gps.alt ] ecef = navpy.lla2ecef(rawgps[:,0],rawgps[:,1],rawgps[:,2],latlon_unit='deg') ref_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref)
def run_filter(imu_data, gps_data): drl = len(imu_data) # result structures estPOS = np.nan * np.ones((drl, 3)) estVEL = np.nan * np.ones((drl, 3)) estATT = np.nan * np.ones((drl, 4)) estAB = np.nan * np.ones((drl, 4)) estGB = np.nan * np.ones((drl, 4)) Pp = np.nan * np.ones((drl, 3)) Pvel = np.nan * np.ones((drl, 3)) Patt = np.nan * np.ones((drl, 3)) Pab = np.nan * np.ones((drl, 3)) Pgb = np.nan * np.ones((drl, 3)) stateInnov = np.nan * np.ones((drl, 6)) plot_time = [0.0] * drl # Variable Initializer idx_init = [] # Main Loop filter = EKF.Filter() gps_index = 1 nav_init = False for i, imu in enumerate(imu_data): # walk the gps counter forward as needed if imu.time >= gps_data[gps_index].time: gps_index += 1 if gps_index >= len(gps_data): # no more gps data, stick on the last record gps_index = len(gps_data) - 1 gps = gps_data[gps_index - 1] # print "t(imu) = " + str(imu.time) + " t(gps) = " + str(gps.time) # update the filter plot_time[i] = imu.time est = filter.update(imu, gps, verbose=False) # save the results for plotting if not nav_init and est.valid: nav_init = True idx_init.append(i) elif not est.valid: nav_init = False estPOS[i, :] = est.estPOS[:] estVEL[i, :] = est.estVEL[:] estATT[i, :] = est.estATT[:] estAB[i, 0:3] = est.estAB[:] estAB[i, 3] = imu.temp estGB[i, 0:3] = est.estGB[:] estGB[i, 3] = imu.temp Pp[i, :] = np.diag(est.P[0:3, 0:3]) Pvel[i, :] = np.diag(est.P[3:6, 3:6]) Patt[i, :] = np.diag(est.P[6:9, 6:9]) Pab[i, :] = np.diag(est.P[9:12, 9:12]) Pgb[i, :] = np.diag(est.P[12:15, 12:15]) stateInnov[i, :] = est.stateInnov[:] psi, theta, phi = navpy.quat2angle(estATT[:, 0], estATT[:, 1:4], output_unit='deg') # Calculate Attitude Error delta_att = np.nan * np.zeros((drl, 3)) for i in range(0, drl): # when processing real data, we have no truth reference #C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T C1 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T C2 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit='deg').T dC = C2.dot(C1.T) - np.eye(3) # dC contains delta angle. To match delta quaternion, divide by 2. delta_att[i, :] = [-dC[1, 2] / 2.0, dC[0, 2] / 2.0, -dC[0, 1] / 2.0] lat_ref = estPOS[idx_init[0], 0] lon_ref = estPOS[idx_init[0], 1] alt_ref = estPOS[idx_init[0], 2] ecef_ref = navpy.lla2ecef(lat_ref, lon_ref, alt_ref) ecef = navpy.lla2ecef(estPOS[:, 0], estPOS[:, 1], estPOS[:, 2]) filter_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref) # when processing real data, we have no truth reference #ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt) #ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) #gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) #gnss_ned[0:idx_init[0],:] = np.nan rawgps = np.nan * np.ones((len(gps_data), 3)) for i, gps in enumerate(gps_data): rawgps[i, :] = [gps.lat, gps.lon, gps.alt] ecef = navpy.lla2ecef(rawgps[:, 0], rawgps[:, 1], rawgps[:, 2], latlon_unit='deg') ref_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref) ref_ned[0:idx_init[0], :] = np.nan return estPOS, estVEL, estATT, estAB, estGB, Pp, Pvel, Patt, Pab, Pgb, stateInnov, plot_time, idx_init, filter_ned, ref_ned, psi, theta, phi, delta_att
def run_filter(imu_data, gps_data): drl = len(imu_data) # result structures estPOS = np.nan * np.ones((drl, 3)) estVEL = np.nan * np.ones((drl, 3)) estATT = np.nan * np.ones((drl, 4)) estAB = np.nan * np.ones((drl, 4)) estGB = np.nan * np.ones((drl, 4)) Pp = np.nan * np.ones((drl, 3)) Pvel = np.nan * np.ones((drl, 3)) Patt = np.nan * np.ones((drl, 3)) Pab = np.nan * np.ones((drl, 3)) Pgb = np.nan * np.ones((drl, 3)) stateInnov = np.nan * np.ones((drl, 6)) plot_time = [0.0] * drl # Variable Initializer idx_init = [] # Main Loop filter = EKF.Filter() gps_index = 1 nav_init = False for i, imu in enumerate(imu_data): # walk the gps counter forward as needed if imu.time >= gps_data[gps_index].time: gps_index += 1 if gps_index >= len(gps_data): # no more gps data, stick on the last record gps_index = len(gps_data) - 1 gps = gps_data[gps_index - 1] # print "t(imu) = " + str(imu.time) + " t(gps) = " + str(gps.time) # update the filter plot_time[i] = imu.time est = filter.update(imu, gps, verbose=False) # save the results for plotting if not nav_init and est.valid: nav_init = True idx_init.append(i) elif not est.valid: nav_init = False estPOS[i, :] = est.estPOS[:] estVEL[i, :] = est.estVEL[:] estATT[i, :] = est.estATT[:] estAB[i, 0:3] = est.estAB[:] estAB[i, 3] = imu.temp estGB[i, 0:3] = est.estGB[:] estGB[i, 3] = imu.temp Pp[i, :] = np.diag(est.P[0:3, 0:3]) Pvel[i, :] = np.diag(est.P[3:6, 3:6]) Patt[i, :] = np.diag(est.P[6:9, 6:9]) Pab[i, :] = np.diag(est.P[9:12, 9:12]) Pgb[i, :] = np.diag(est.P[12:15, 12:15]) stateInnov[i, :] = est.stateInnov[:] psi, theta, phi = navpy.quat2angle(estATT[:, 0], estATT[:, 1:4], output_unit="deg") # Calculate Attitude Error delta_att = np.nan * np.zeros((drl, 3)) for i in range(0, drl): # when processing real data, we have no truth reference # C1 = navpy.angle2dcm(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]).T C1 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit="deg").T C2 = navpy.angle2dcm(psi[i], theta[i], phi[i], input_unit="deg").T dC = C2.dot(C1.T) - np.eye(3) # dC contains delta angle. To match delta quaternion, divide by 2. delta_att[i, :] = [-dC[1, 2] / 2.0, dC[0, 2] / 2.0, -dC[0, 1] / 2.0] lat_ref = estPOS[idx_init[0], 0] lon_ref = estPOS[idx_init[0], 1] alt_ref = estPOS[idx_init[0], 2] ecef_ref = navpy.lla2ecef(lat_ref, lon_ref, alt_ref) ecef = navpy.lla2ecef(estPOS[:, 0], estPOS[:, 1], estPOS[:, 2]) filter_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref) # when processing real data, we have no truth reference # ecef = navpy.lla2ecef(flight_data.lat,flight_data.lon,flight_data.alt) # ecef = navpy.lla2ecef(estPOS[:,0],estPOS[:,1],estPOS[:,2]) # gnss_ned = navpy.ecef2ned(ecef-ecef_ref,lat_ref,lon_ref,alt_ref) # gnss_ned[0:idx_init[0],:] = np.nan rawgps = np.nan * np.ones((len(gps_data), 3)) for i, gps in enumerate(gps_data): rawgps[i, :] = [gps.lat, gps.lon, gps.alt] ecef = navpy.lla2ecef(rawgps[:, 0], rawgps[:, 1], rawgps[:, 2], latlon_unit="deg") ref_ned = navpy.ecef2ned(ecef - ecef_ref, lat_ref, lon_ref, alt_ref) ref_ned[0 : idx_init[0], :] = np.nan return ( estPOS, estVEL, estATT, estAB, estGB, Pp, Pvel, Patt, Pab, Pgb, stateInnov, plot_time, idx_init, filter_ned, ref_ned, psi, theta, phi, delta_att, )
def update(self, imu, gps, verbose=True): # Check for validity of GNSS at this time self.tcpu = imu.time self.tow = gps.tow # Test 1: navValid flag and the data is indeed new (new Time of Week) # Execute Measurement Update if this is true and Test 2 passes NEW_GNSS_FLAG = ((gps.valid==0) & (abs(self.tow-self.last_tow) > 1e-3)) # Test 2: Check if the delta time of the Time of Week and the # CPU time are consistent # If this fails, re-initialize the filter. There must have been a glitch if NEW_GNSS_FLAG: #print self.tcpu, self.last_tcpu, self.tcpu-self.last_tcpu #print self.tow, self.last_tow, self.tow-self.last_tow if abs((self.tcpu-self.last_tcpu) - (self.tow-self.last_tow)) > 0.5: self.TU_COUNT = 0 self.NAV_INIT = False if verbose: print("Time Sync Error -- Request reinitialization") # Record the tow and tcpu at this new update self.last_tow = self.tow self.last_tcpu = self.tcpu # Different subroutine executed in the filter if not self.NAV_INIT: if not self.IMU_CAL_INIT: # SUBROUTINE: IMU CALIBRATION, # This only happens first time round on the ground. Inflight # reinitialization is not the same. self.estAB[:] = [0,0,0] if self.very_first_time: self.very_first_time = False self.estGB[:] = [imu.p, imu.q, imu.r] self.phi = 0 self.theta = 0 else: self.estGB[:] = self.last_estGB[:]*self.TU_COUNT \ + [imu.p, imu.q, imu.r] # Simple AHRS values self.phi = self.phi*self.TU_COUNT \ + np.arctan2(-imu.ay, -imu.az) self.theta = self.theta*self.TU_COUNT \ + np.arctan2(imu.ax, np.sqrt(imu.ay**2+imu.az**2)) self.phi /= (self.TU_COUNT + 1) self.theta /= (self.TU_COUNT + 1) self.estGB[:] /= (self.TU_COUNT + 1) self.estATT[0], self.estATT[1:] = navpy.angle2quat(0, self.theta, self.phi) """ print("t = %7.3f, Gyro Bias Value: [%6.2f, %6.2f, %6.2f] deg/sec" %\ (imu.time, np.rad2deg(self.estGB[0]), np.rad2deg(self.estGB[1]), np.rad2deg(self.estGB[2]) )) print("t = %7.3f, phi = %6.2f, theta = %6.2f" % (imu.time,np.rad2deg(self.phi),np.rad2deg(self.theta)) ) """ self.TU_COUNT += 1 if self.TU_COUNT >= 35: self.TU_COUNT = 0 self.IMU_CAL_INIT = True if verbose: print("t = %7.3f, IMU Calibrated!" % (imu.time)) del(self.phi) del(self.theta) else: if not NEW_GNSS_FLAG: # SUBROUTINE 1: BACKUP NAVIGATION or IN-FLIGHT INITIALIZATION # >>>> AHRS CODE GOES HERE self.estATT[:] = self.last_estATT[:] # This should be some backup nav mode # <<<< # When still there is no GNSS signal, continue propagating bias self.estAB[:] = self.last_estAB[:] self.estGB[:] = self.last_estGB[:] else: # When there is GNSS fix available, initialize all the states # and renew covariance self.estPOS[:] = [gps.lat, gps.lon, gps.alt] self.estVEL[:] = [gps.vn, gps.ve, gps.vd] self.estATT[:] = self.last_estATT[:] #self.estATT[0], self.estATT[1:] = navpy.angle2quat(flight_data.psi[i],flight_data.theta[i],flight_data.phi[i]) self.estAB[:] = self.last_estAB[:] self.estGB[:] = self.last_estGB[:] self.init_covariance() #idx_init.append(i) self.NAV_INIT = True if verbose: print("t = %7.3f, Filter (Re-)initialized" % (imu.time) ) elif self.NAV_INIT: # SUBROUTINE 2: MAIN FILTER ALGORITHM, INS + GNSS # ==== Time-Update ==== dt = imu.time - self.last_imu.time q0, qvec = self.last_estATT[0], self.last_estATT[1:4] C_B2N = navpy.quat2dcm(q0,qvec).T # 0. Data Acquisition f_b=np.array([0.5*(self.last_imu.ax+imu.ax)-self.last_estAB[0],\ 0.5*(self.last_imu.ay+imu.ay)-self.last_estAB[1],\ 0.5*(self.last_imu.az+imu.az)-self.last_estAB[2]]) om_ib=np.array([0.5*(self.last_imu.p+imu.p)-self.last_estGB[0],\ 0.5*(self.last_imu.q+imu.q)-self.last_estGB[1],\ 0.5*(self.last_imu.r+imu.r)-self.last_estGB[2]]) # 1. Attitude Update # --> Need to compensate for navrate and earthrate dqvec = 0.5*om_ib*dt self.estATT[0], self.estATT[1:4] = navpy.qmult(q0,qvec,1.0,dqvec) self.estATT[0] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) # 2. Velocity Update # --> Need to compensate for coriolis effect g = np.array([0,0,9.81]) f0, fvec = navpy.qmult(q0,qvec,0,f_b) f_n0,f_nvec = navpy.qmult(f0,fvec,q0,-qvec) self.estVEL[:] = self.last_estVEL[:] + (f_nvec+g)*dt # 3. Position Update dPOS = navpy.llarate(self.last_estVEL[0], self.last_estVEL[1], self.last_estVEL[2], self.last_estPOS[0], self.last_estPOS[2]) dPOS *= dt self.estPOS[:] = self.last_estPOS[:] + dPOS # 4. Biases are constant self.estAB[:] = self.last_estAB[:] self.estGB[:] = self.last_estGB[:] # 5. Jacobian pos2pos = np.zeros((3,3)) pos2gs = np.eye(3) pos2att = np.zeros((3,3)) pos2acc = np.zeros((3,3)) pos2gyr = np.zeros((3,3)) gs2pos = np.zeros((3,3)) gs2pos[2,2] = -2*9.81/wgs84.R0 gs2gs = np.zeros((3,3)) gs2att = -2*C_B2N.dot(navpy.skew(f_b)) gs2acc = -C_B2N gs2gyr = np.zeros((3,3)) att2pos = np.zeros((3,3)) att2gs = np.zeros((3,3)) att2att = -navpy.skew(om_ib) att2acc = np.zeros((3,3)) att2gyr = -0.5*np.eye(3) F = np.zeros((15,15)) F[0:3,0:3] = pos2pos F[0:3,3:6] = pos2gs F[0:3,6:9] = pos2att F[0:3,9:12] = pos2acc F[0:3,12:15] = pos2gyr F[3:6,0:3] = gs2pos F[3:6,3:6] = gs2gs F[3:6,6:9] = gs2att F[3:6,9:12] = gs2acc F[3:6,12:15] = gs2gyr F[6:9,0:3] = att2pos F[6:9,3:6] = att2gs F[6:9,6:9] = att2att F[6:9,9:12] = att2acc F[6:9,12:15] = att2gyr F[9:12,9:12] = -1.0/self.tau_a*np.eye(3) F[12:15,12:15] = -1.0/self.tau_g*np.eye(3) PHI = np.eye(15) + F*dt # 6. Process Noise G = np.zeros((15,12)) G[3:6,0:3] = -C_B2N G[6:9,3:6] = att2gyr G[9:12,6:9] = np.eye(3) G[12:15,9:12] = np.eye(3) Q = G.dot(self.Rw.dot(G.T))*dt # 7. Covariance Update self.P = PHI.dot(self.P.dot(PHI.T)) + Q self.TU_COUNT += 1 if self.TU_COUNT >= 500: # Request reinitialization after 12 seconds of no GNSS updates self.TU_COUNT = 0 self.NAV_INIT = False if NEW_GNSS_FLAG: # ==== Measurement-Update ==== # 0. Get measurement and make innovations ecef_ref = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], 0) ins_ecef = navpy.lla2ecef(self.estPOS[0], self.estPOS[1], self.estPOS[2]) gnss_ecef = navpy.lla2ecef(gps.lat, gps.lon, gps.alt) ins_ned = navpy.ecef2ned(ins_ecef-ecef_ref, self.estPOS[0], self.estPOS[1], self.estPOS[2]) gnss_ned = navpy.ecef2ned(gnss_ecef-ecef_ref, self.estPOS[0], self.estPOS[1], self.estPOS[2]) dpos = gnss_ned - ins_ned gnss_vel = np.array([gps.vn, gps.ve, gps.vd]) dvel = gnss_vel - self.estVEL[:] dy = np.hstack((dpos, dvel)) self.stateInnov[:] = dy # 1. Kalman Gain K = self.P.dot(self.H.T) K = K.dot( la.inv(self.H.dot(self.P.dot(self.H.T)) + self.R) ) # 2. Covariance Update ImKH = np.eye(15) - K.dot(self.H) KRKt = K.dot(self.R.dot(K.T)) self.P = ImKH.dot(self.P.dot(ImKH.T)) + KRKt # 3. State Update dx = K.dot(dy) Rew, Rns = navpy.earthrad(self.estPOS[0]) self.estPOS[2] -= dx[2] self.estPOS[0] += np.rad2deg(dx[0]/(Rew + self.estPOS[2])) self.estPOS[1] += np.rad2deg(dx[1]/(Rns + self.estPOS[2])/np.cos(np.deg2rad(self.estPOS[0]))) self.estVEL[:] += dx[3:6] self.estATT[0], self.estATT[1:4] = navpy.qmult(self.estATT[0], self.estATT[1:4], 1, dx[6:9]) self.estATT[0] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) self.estATT[1:4] /= np.sqrt(self.estATT[0]**2 \ + la.norm(self.estATT[1:4])**2) self.estAB[:] += dx[9:12] self.estGB[:] += dx[12:15] if verbose: print("t = %7.3f, GNSS Update, self.TU_COUNT = %d" %\ (gps.time, self.TU_COUNT) ) self.TU_COUNT = 0 self.last_estPOS[:] = self.estPOS[:] self.last_estVEL[:] = self.estVEL[:] self.last_estATT[:] = self.estATT[:] self.last_estAB[:] = self.estAB[:] self.last_estGB[:] = self.estGB[:] else: # SUBROUTINE 3: e.g. BACKUP NAVIGATION MODE pass self.last_imu = imu self.last_gps = gps result = INSGPS( self.NAV_INIT, imu.time, self.estPOS, self.estVEL, self.estATT, self.estAB, self.estGB, self.P, self.stateInnov ) return result