zbw/yiliao2026
1
0
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

零漂数据采集拟合

Browse Source
This commit is contained in:
Orange
2000-01-01 08:38:51 +08:00
parent b14befd1cf
commit 2f51de27e5
6 changed files with 1430 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
scripts/base_feedback_monitor.py
View File

@@ -66,9 +66,9 @@ class BaseFeedbackMonitor(Node):
"robotvel_x",
"robotvel_y",
"robotvel_z",
"gyro_raw_z_rad_s",
"gyro_bias_fit_z_rad_s",
"gyro_corrected_z_rad_s",
"gyro_z_filtered_pre_bias",
"gyro_z_bias_model",
"gyro_z_final_for_yaw",
])
self.start_time = self.get_clock().now()
@@ -169,9 +169,9 @@ class BaseFeedbackMonitor(Node):
"robotvel_x": self.robotvel_msg.x if self.robotvel_msg is not None else None,
"robotvel_y": self.robotvel_msg.y if self.robotvel_msg is not None else None,
"robotvel_z": self.robotvel_msg.z if self.robotvel_msg is not None else None,
"gyro_raw_z_rad_s": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
"gyro_bias_fit_z_rad_s": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
"gyro_corrected_z_rad_s": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
"gyro_z_filtered_pre_bias": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
"gyro_z_bias_model": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
"gyro_z_final_for_yaw": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
}
def log_snapshot(self) -> None:
@@ -270,9 +270,9 @@ class BaseFeedbackMonitor(Node):
gyro = self.gyrodebug_msg
sys.stdout.write(
"gyro debug: "
f"raw_z={self._fmt_float(gyro.x, 6)} "
f"pre_bias={self._fmt_float(gyro.x, 6)} "
f"bias_z={self._fmt_float(gyro.y, 6)} "
f"corr_z={self._fmt_float(gyro.z, 6)} rad/s\n"
f"final_z={self._fmt_float(gyro.z, 6)} rad/s\n"
)
else:
sys.stdout.write("gyro debug: N/A\n")

6
scripts/fit_gyro_bias.py
View File

@@ -13,12 +13,14 @@ def load_samples(csv_path: Path):
with csv_path.open("r", encoding="utf-8", newline="") as f:
reader = csv.DictReader(f)
fieldnames = reader.fieldnames or []
if "gyro_raw_z_rad_s" in fieldnames:
if "gyro_z_filtered_pre_bias" in fieldnames:
source_name = "gyro_z_filtered_pre_bias"
elif "gyro_raw_z_rad_s" in fieldnames:
source_name = "gyro_raw_z_rad_s"
elif "imu_angular_velocity_z" in fieldnames:
source_name = "imu_angular_velocity_z"
else:
raise ValueError("CSV does not contain gyro_raw_z_rad_s or imu_angular_velocity_z")
raise ValueError("CSV does not contain gyro_z_filtered_pre_bias, gyro_raw_z_rad_s, or imu_angular_velocity_z")
for row in reader:
t = row.get("elapsed_s")
z = row.get(source_name)