def __init__(self, file_name, directory, **plot_info):
project.set_project_dir("roboquasar")
self.checkpoints = get_map("buggy course checkpoints.gpx")
self.course_map = get_map("buggy course map.gpx")
super(FilterTest, self).__init__(
file_name, directory, 1, plot_info
)
first_gps = self.parser.get(5, "gps")[-1]
second_gps = self.parser.get(50, "gps")[-1]
lat1, long1, alt1 = first_gps["lat"], first_gps["long"], first_gps["altitude"]
lat2, long2, alt2 = second_gps["lat"], second_gps["long"], second_gps["altitude"]
print(lat1, long1, alt1)
print(lat2, long2, alt2)
initial_yaw, initial_pitch, initial_roll = \
get_gps_orientation(lat1, long1, alt1,
lat2, long2, alt2)
print(np.array([initial_yaw, initial_pitch, initial_roll]) * 180 / np.pi)
self.filter = GrovesKalmanFilter(
initial_roll_ecef=initial_roll,
initial_pitch_ecef=initial_pitch,
initial_yaw_ecef=initial_yaw,
initial_lat=lat1,
initial_long=long1,
initial_alt=alt1,
**constants)
first_gps_reading = self.parser.get(0, 'gps')[-1]
initial_lat = first_gps_reading["lat"]
initial_long = first_gps_reading["long"]
self.draw_starting_dot(initial_long, initial_lat)
self.prev_lat, self.prev_long = initial_lat, initial_long
self.prev_imu_t = 0
self.prev_gps_t = 0
class FilterTest(Simulator):
def __init__(self, file_name, directory, **plot_info):
project.set_project_dir("roboquasar")
self.checkpoints = get_map("buggy course checkpoints.gpx")
self.course_map = get_map("buggy course map.gpx")
super(FilterTest, self).__init__(
file_name, directory, 1, plot_info
)
first_gps = self.parser.get(5, "gps")[-1]
second_gps = self.parser.get(50, "gps")[-1]
lat1, long1, alt1 = first_gps["lat"], first_gps["long"], first_gps["altitude"]
lat2, long2, alt2 = second_gps["lat"], second_gps["long"], second_gps["altitude"]
print(lat1, long1, alt1)
print(lat2, long2, alt2)
initial_yaw, initial_pitch, initial_roll = \
get_gps_orientation(lat1, long1, alt1,
lat2, long2, alt2)
print(np.array([initial_yaw, initial_pitch, initial_roll]) * 180 / np.pi)
self.filter = GrovesKalmanFilter(
initial_roll_ecef=initial_roll,
initial_pitch_ecef=initial_pitch,
initial_yaw_ecef=initial_yaw,
initial_lat=lat1,
initial_long=long1,
initial_alt=alt1,
**constants)
first_gps_reading = self.parser.get(0, 'gps')[-1]
initial_lat = first_gps_reading["lat"]
initial_long = first_gps_reading["long"]
self.draw_starting_dot(initial_long, initial_lat)
self.prev_lat, self.prev_long = initial_lat, initial_long
self.prev_imu_t = 0
self.prev_gps_t = 0
def fill_data_array(self, plot_option, data_array):
if plot_option == "checkpoints_plot":
checkpoints_map = np.array(self.checkpoints)
data_array[0] = checkpoints_map[:, 1]
data_array[1] = checkpoints_map[:, 0]
elif plot_option == "course_map_plot":
course_map = np.array(self.course_map)
data_array[0] = course_map[:, 1]
data_array[1] = course_map[:, 0]
def step(self, index, timestamp, name, values):
if name == "imu":
self.record_imu(timestamp, values)
elif name == "gps":
self.record_gps(timestamp, values)
def record_imu(self, timestamp, values):
if self.plot_enabled["imu_plot"]:
self.angled_line_segment("imu_plot", self.prev_long, self.prev_lat,
values['yaw'], 0.5)
if self.plot_enabled["calculated_filter_plot"]:
self.filter.imu_updated(
timestamp - self.prev_imu_t,
values["ax"], values["ay"], values["az"],
values["gx"], values["gy"], values["gz"],
)
self.prev_imu_t = timestamp
self.record_position()
def record_gps(self, timestamp, values):
if self.plot_enabled["gps_plot"]:
self.plot_data["gps_plot"][0].append(values["long"])
self.plot_data["gps_plot"][1].append(values["lat"])
self.prev_lat = values["lat"]
self.prev_long = values["long"]
if self.plot_enabled["calculated_filter_plot"]:
self.filter.gps_updated(
timestamp - self.prev_gps_t,
values["lat"], values["long"], values["altitude"]
)
self.prev_gps_t = timestamp
self.record_position()
def record_position(self):
lat, long, height = self.filter.get_position()
self.plot_data["calculated_filter_plot"][0].append(long)
self.plot_data["calculated_filter_plot"][1].append(lat)
yaw, pitch, roll = self.filter.get_orientation()
if self.plot_enabled["calculated_filter_heading_plot"]:
self.angled_line_segment("calculated_filter_heading_plot", long, lat, yaw, 0.5)
from autobuggy.filters.kalman_filter import GrovesKalmanFilter
from roboquasar_constants import constants
kf = GrovesKalmanFilter(**constants)
kf.imu_updated(0.02098393440246582,
0.019999999552965164, 0.07999999821186066, -0.2800000011920929,
-0.04886922240257263, 0.13962635397911072, -0.027925269678235054)
kf.gps_updated(0.03800225257873535,
40.44057846069336, -79.94245147705078, 302.79998779296875)
print(kf.get_position())
