First_Dev_Dept/IMU
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: add fixed yaw bias compensation

Browse Source
This commit is contained in:
zequn.liu
2026-06-18 14:41:08 +08:00
parent 97cd9a56c6
commit 93b9b91db7
2 changed files with 355 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
scripts/run_imu_ekf.py
+173 -33
View File
@@ -61,9 +61,11 @@ def process_file(
input_csv: Path,
output_dir: Path,
init_seconds: int = 3,
yaw_bias_seconds: int = 60,
max_points: int = 2500,
) -> EkfFileResult:
init_seconds = _validate_init_seconds(init_seconds)
yaw_bias_seconds = _validate_yaw_bias_seconds(yaw_bias_seconds)
input_csv = Path(input_csv)
output_dir = Path(output_dir)
output_dir.mkdir(parents=True, exist_ok=True)
@@ -90,6 +92,7 @@ def process_file(
"gyro_bias_x_dps",
"gyro_bias_y_dps",
"gyro_bias_z_dps",
"fixed_yaw_bias_z_dps",
"acc_residual_norm",
"dt_s",
"acc_update_used",
@@ -99,16 +102,22 @@ def process_file(
segment_id = 0
state: ekf.ImuEkfState | None = None
yaw_bias_buffer: list[ImuRow] = []
init_buffer: list[ImuRow] = []
fixed_yaw_bias_enabled = yaw_bias_seconds > 0
fixed_yaw_bias_z_dps: float | None = 0.0 if yaw_bias_seconds == 0 else None
previous_input_time: float | None = None
previous_output_time: float | None = None
relative_yaw_deg = 0.0
previous_yaw_deg: float | None = None
def reset_segment() -> None:
nonlocal state, init_buffer, previous_output_time, relative_yaw_deg, previous_yaw_deg
nonlocal state, yaw_bias_buffer, init_buffer, fixed_yaw_bias_z_dps
nonlocal previous_output_time, relative_yaw_deg, previous_yaw_deg
state = None
yaw_bias_buffer = []
init_buffer = []
fixed_yaw_bias_z_dps = 0.0 if yaw_bias_seconds == 0 else None
previous_output_time = None
relative_yaw_deg = 0.0
previous_yaw_deg = None
@@ -117,10 +126,16 @@ def process_file(
nonlocal input_rows, previous_output_time, relative_yaw_deg, previous_yaw_deg
if state is None:
raise ValueError("EKF state is not initialized")
if fixed_yaw_bias_z_dps is None:
raise ValueError("fixed yaw bias is not initialized")
if fixed_yaw_bias_enabled:
state.gyro_bias_rad_s[2] = 0.0
dt_s = 0.0 if previous_output_time is None else row.sensor_uptime_s - previous_output_time
previous_output_time = row.sensor_uptime_s
used_update, residual_norm = ekf.step(state, dt_s, _acc_mps2(row), _gyro_rad_s(row))
if fixed_yaw_bias_enabled:
state.gyro_bias_rad_s[2] = 0.0
roll, pitch, yaw = ekf.quaternion_to_euler_deg(state.q)
if previous_yaw_deg is None:
relative_yaw_deg = 0.0
@@ -143,6 +158,7 @@ def process_file(
"gyro_bias_x_dps": bias_dps[0],
"gyro_bias_y_dps": bias_dps[1],
"gyro_bias_z_dps": bias_dps[2],
"fixed_yaw_bias_z_dps": fixed_yaw_bias_z_dps,
"acc_residual_norm": residual_norm,
"dt_s": dt_s,
"acc_update_used": int(used_update),
@@ -155,17 +171,47 @@ def process_file(
def initialize_and_write_buffer() -> None:
nonlocal state, init_buffer
state = _initialize_state(init_buffer, init_seconds)
if fixed_yaw_bias_enabled:
state.gyro_bias_rad_s[2] = 0.0
buffered_rows = init_buffer
init_buffer = []
for buffered_row in buffered_rows:
write_row(buffered_row)
def process_row_with_fixed_yaw_bias(row: ImuRow) -> None:
if fixed_yaw_bias_z_dps is None:
raise ValueError("fixed yaw bias is not initialized")
corrected_row = _row_with_fixed_yaw_bias(row, fixed_yaw_bias_z_dps)
if state is None:
init_buffer.append(corrected_row)
if init_seconds == 0 or corrected_row.sensor_uptime_s - init_buffer[0].sensor_uptime_s >= init_seconds:
initialize_and_write_buffer()
else:
write_row(corrected_row)
def initialize_fixed_yaw_bias_from_buffer() -> None:
nonlocal yaw_bias_buffer, fixed_yaw_bias_z_dps
if fixed_yaw_bias_z_dps is not None:
return
if not yaw_bias_buffer:
raise ValueError("at least one IMU row is required for fixed yaw bias initialization")
fixed_yaw_bias_z_dps = _fixed_yaw_bias_z_dps(yaw_bias_buffer)
buffered_rows = yaw_bias_buffer
yaw_bias_buffer = []
for buffered_row in buffered_rows:
process_row_with_fixed_yaw_bias(buffered_row)
def flush_segment() -> None:
if fixed_yaw_bias_z_dps is None and yaw_bias_buffer:
initialize_fixed_yaw_bias_from_buffer()
if state is None and init_buffer:
initialize_and_write_buffer()
for data_row_index, row in enumerate(iter_imu_rows(input_csv), start=1):
rows_seen += 1
if previous_input_time is not None and row.sensor_uptime_s < previous_input_time:
if _is_device_restart(previous_input_time, row.sensor_uptime_s):
if state is None and init_buffer:
initialize_and_write_buffer()
flush_segment()
segment_id += 1
reset_segment()
else:
@@ -174,19 +220,20 @@ def process_file(
f"previous {previous_input_time}, current {row.sensor_uptime_s}"
)
if state is None:
init_buffer.append(row)
if init_seconds == 0 or row.sensor_uptime_s - init_buffer[0].sensor_uptime_s >= init_seconds:
initialize_and_write_buffer()
if fixed_yaw_bias_z_dps is None:
if yaw_bias_buffer and row.sensor_uptime_s - yaw_bias_buffer[0].sensor_uptime_s >= yaw_bias_seconds:
initialize_fixed_yaw_bias_from_buffer()
process_row_with_fixed_yaw_bias(row)
else:
yaw_bias_buffer.append(row)
else:
write_row(row)
process_row_with_fixed_yaw_bias(row)
previous_input_time = row.sensor_uptime_s
if rows_seen == 0:
raise ValueError(f"{input_csv} has no IMU rows")
if state is None and init_buffer:
initialize_and_write_buffer()
flush_segment()
return EkfFileResult(
input_csv=input_csv,
@@ -227,7 +274,7 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
h2 {{ font-size: 18px; margin: 20px 0 8px; }}
.meta {{ color: #5f6368; font-size: 13px; }}
.file {{ margin-bottom: 26px; }}
canvas {{ width: 100%; height: 260px; display: block; background: #ffffff; border: 1px solid #d8d8d0; }}
canvas.chart {{ width: 100%; height: 390px; display: block; background: #ffffff; border: 1px solid #b8bec5; }}
table {{ border-collapse: collapse; margin: 10px 0; font-size: 13px; }}
td {{ border: 1px solid #d8d8d0; padding: 4px 8px; }}
</style>
@@ -241,37 +288,83 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
<script id="ekf-data" type="application/json">{data_json}</script>
<script>
const colors = {{ roll_deg: '#b3261e', pitch_deg: '#146c2e', relative_yaw_deg: '#1a73e8' }};
const labels = {{ roll_deg: 'roll deg', pitch_deg: 'pitch deg', relative_yaw_deg: 'relative yaw deg' }};
const redraws = [];
function drawChart(canvas, rows, fields) {{
const ctx = canvas.getContext('2d');
const w = canvas.width = canvas.clientWidth * devicePixelRatio;
const h = canvas.height = canvas.clientHeight * devicePixelRatio;
ctx.scale(devicePixelRatio, devicePixelRatio);
const width = canvas.clientWidth;
const height = canvas.clientHeight;
const dpr = window.devicePixelRatio || 1;
const rect = canvas.getBoundingClientRect();
const width = Math.max(320, Math.floor(rect.width || canvas.clientWidth || 640));
const height = Math.max(260, Math.floor(rect.height || canvas.clientHeight || 390));
canvas.width = Math.floor(width * dpr);
canvas.height = Math.floor(height * dpr);
ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
ctx.clearRect(0, 0, width, height);
ctx.strokeStyle = '#d8d8d0';
ctx.strokeRect(40, 12, width - 52, height - 40);
if (!rows.length) return;
const xs = rows.map(r => r.sensor_uptime_s);
const ys = rows.flatMap(r => fields.map(f => r[f]));
ctx.fillStyle = '#ffffff';
ctx.fillRect(0, 0, width, height);
const finiteRows = rows.filter(row =>
Number.isFinite(row.sensor_uptime_s) && fields.every(field => Number.isFinite(row[field]))
);
if (!finiteRows.length) {{
ctx.fillStyle = '#5f6368';
ctx.fillText('No finite samples to draw', 12, 24);
return;
}}
const xs = finiteRows.map(row => row.sensor_uptime_s);
const xmin = Math.min(...xs), xmax = Math.max(...xs);
const ymin = Math.min(...ys), ymax = Math.max(...ys);
const xspan = Math.max(xmax - xmin, 1e-9);
const yspan = Math.max(ymax - ymin, 1e-9);
for (const field of fields) {{
const left = 62;
const right = 14;
const top = 18;
const bottom = 22;
const gap = 18;
const plotWidth = Math.max(1, width - left - right);
const laneHeight = Math.max(40, (height - top - bottom - gap * (fields.length - 1)) / fields.length);
fields.forEach((field, fieldIndex) => {{
const laneTop = top + fieldIndex * (laneHeight + gap);
const values = finiteRows.map(row => row[field]);
let ymin = Math.min(...values);
let ymax = Math.max(...values);
if (Math.abs(ymax - ymin) < 1e-9) {{
ymin -= 1;
ymax += 1;
}}
const yspan = ymax - ymin;
ctx.strokeStyle = '#d5d9de';
ctx.lineWidth = 1;
ctx.strokeRect(left, laneTop, plotWidth, laneHeight);
ctx.beginPath();
ctx.moveTo(left, laneTop + laneHeight / 2);
ctx.lineTo(left + plotWidth, laneTop + laneHeight / 2);
ctx.stroke();
ctx.fillStyle = colors[field] || '#444444';
ctx.font = '12px Arial, sans-serif';
ctx.fillText(labels[field] || field, 8, laneTop + 13);
ctx.fillStyle = '#5f6368';
ctx.fillText(ymax.toFixed(2), 8, laneTop + 29);
ctx.fillText(ymin.toFixed(2), 8, laneTop + laneHeight - 4);
ctx.beginPath();
ctx.strokeStyle = colors[field] || '#444';
rows.forEach((r, i) => {{
const x = 40 + ((r.sensor_uptime_s - xmin) / xspan) * (width - 52);
const y = 12 + (1 - ((r[field] - ymin) / yspan)) * (height - 40);
ctx.lineWidth = 2;
finiteRows.forEach((r, i) => {{
const x = left + ((r.sensor_uptime_s - xmin) / xspan) * plotWidth;
const y = laneTop + (1 - ((r[field] - ymin) / yspan)) * laneHeight;
if (i === 0) ctx.moveTo(x, y); else ctx.lineTo(x, y);
}});
ctx.stroke();
}}
}});
ctx.fillStyle = '#5f6368';
ctx.fillText(ymax.toFixed(2), 4, 20);
ctx.fillText(ymin.toFixed(2), 4, height - 28);
ctx.font = '12px Arial, sans-serif';
ctx.fillText(xmin.toFixed(2) + 's', left, height - 6);
ctx.fillText(xmax.toFixed(2) + 's', Math.max(left, width - right - 80), height - 6);
}}
function render() {{
@@ -287,7 +380,11 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
const summary = document.createElement('div');
summary.className = 'meta';
summary.textContent = `Rows: ${{file.input_rows}} | Output: ${{file.output_csv}}`;
const lastSample = file.samples.length ? file.samples[file.samples.length - 1] : null;
const fixedYawBias = lastSample && Number.isFinite(lastSample.fixed_yaw_bias_z_dps)
? ` | fixed yaw bias z: ${{lastSample.fixed_yaw_bias_z_dps.toFixed(5)}} dps`
: '';
summary.textContent = `Rows: ${{file.input_rows}} | Output: ${{file.output_csv}}${{fixedYawBias}}`;
section.appendChild(summary);
const table = document.createElement('table');
@@ -304,6 +401,7 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
section.appendChild(table);
const canvas = document.createElement('canvas');
canvas.className = 'chart';
section.appendChild(canvas);
const legend = document.createElement('div');
@@ -312,8 +410,10 @@ def write_html_report(results: list[EkfFileResult], html_path: Path, max_points:
section.appendChild(legend);
app.appendChild(section);
drawChart(canvas, file.samples, ['roll_deg', 'pitch_deg', 'relative_yaw_deg']);
redraws.push(() => drawChart(canvas, file.samples, ['roll_deg', 'pitch_deg', 'relative_yaw_deg']));
}}
requestAnimationFrame(() => redraws.forEach(redraw => redraw()));
window.addEventListener('resize', () => redraws.forEach(redraw => redraw()));
}}
render();
</script>
@@ -328,6 +428,7 @@ def main(argv: list[str] | None = None) -> int:
parser.add_argument("csv_files", nargs="*", type=Path, help="CSV files. Defaults to imu_*.csv.")
parser.add_argument("--output-dir", type=Path, default=Path("output") / "ekf")
parser.add_argument("--init-seconds", type=_parse_init_seconds, default=3)
parser.add_argument("--yaw-bias-seconds", type=_parse_yaw_bias_seconds, default=60)
parser.add_argument("--max-points", type=int, default=2500)
args = parser.parse_args(argv)
@@ -336,7 +437,13 @@ def main(argv: list[str] | None = None) -> int:
raise SystemExit("No CSV files found.")
results = [
process_file(path, args.output_dir, init_seconds=args.init_seconds, max_points=args.max_points)
process_file(
path,
args.output_dir,
init_seconds=args.init_seconds,
yaw_bias_seconds=args.yaw_bias_seconds,
max_points=args.max_points,
)
for path in csv_files
]
write_html_report(results, args.output_dir / "ekf_viewer.html", max_points=args.max_points)
@@ -414,6 +521,12 @@ def _validate_init_seconds(value) -> int:
return value
def _validate_yaw_bias_seconds(value) -> int:
if type(value) is not int or value < 0:
raise ValueError("yaw_bias_seconds must be a non-negative integer")
return value
def _parse_init_seconds(value: str) -> int:
try:
parsed = int(value)
@@ -427,6 +540,19 @@ def _parse_init_seconds(value: str) -> int:
raise argparse.ArgumentTypeError(str(exc)) from exc
def _parse_yaw_bias_seconds(value: str) -> int:
try:
parsed = int(value)
except ValueError as exc:
raise argparse.ArgumentTypeError("yaw_bias_seconds must be a non-negative integer") from exc
if str(parsed) != value:
raise argparse.ArgumentTypeError("yaw_bias_seconds must be a non-negative integer")
try:
return _validate_yaw_bias_seconds(parsed)
except ValueError as exc:
raise argparse.ArgumentTypeError(str(exc)) from exc
def _initialize_state(rows: list[ImuRow], init_seconds: int) -> ekf.ImuEkfState:
if not rows:
raise ValueError("at least one IMU row is required for initialization")
@@ -445,6 +571,19 @@ def _gyro_rad_s(row: ImuRow) -> np.ndarray:
return np.radians(np.array(row.gyro_dps, dtype=float))
def _fixed_yaw_bias_z_dps(rows: list[ImuRow]) -> float:
return sum(row.gyro_dps[2] for row in rows) / len(rows)
def _row_with_fixed_yaw_bias(row: ImuRow, fixed_yaw_bias_z_dps: float) -> ImuRow:
return ImuRow(
sensor_uptime_s=row.sensor_uptime_s,
temp_c=row.temp_c,
acc_g=row.acc_g,
gyro_dps=(row.gyro_dps[0], row.gyro_dps[1], row.gyro_dps[2] - fixed_yaw_bias_z_dps),
)
def _sample_for_html(row: dict[str, float]) -> dict[str, float]:
keys = (
"sensor_uptime_s",
@@ -454,6 +593,7 @@ def _sample_for_html(row: dict[str, float]) -> dict[str, float]:
"gyro_bias_x_dps",
"gyro_bias_y_dps",
"gyro_bias_z_dps",
"fixed_yaw_bias_z_dps",
"acc_residual_norm",
)
sample = {key: float(row[key]) for key in keys}