721 lines
30 KiB
Python
721 lines
30 KiB
Python
#!/usr/bin/env python3
|
||
"""Offline GPS/IMU fusion for the stationary Genesys2 capture.
|
||
|
||
The two UART logs are captured sequentially, so their elapsed-time axes are
|
||
replayed together. That is valid for the explicitly stationary experiment,
|
||
but is not a substitute for a synchronous moving-platform capture.
|
||
|
||
The IMU parser accepts both the original averaged ICM-4268x report and the
|
||
timestamped raw LSM6DSOX stream from ``steval_mki217v1_test``.
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import csv
|
||
import json
|
||
import math
|
||
import re
|
||
from dataclasses import dataclass
|
||
from pathlib import Path
|
||
|
||
import matplotlib
|
||
|
||
matplotlib.use("Agg")
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
|
||
|
||
G = 9.80665
|
||
|
||
GPS_RE = re.compile(
|
||
r"position lat=(?P<lat>[-+0-9.eE]+) lon=(?P<lon>[-+0-9.eE]+) "
|
||
r"height=(?P<height>[-+0-9.eE]+)"
|
||
)
|
||
DOP_RE = re.compile(
|
||
r"DOP GDOP=(?P<gdop>[-+0-9.eE]+) PDOP=(?P<pdop>[-+0-9.eE]+) "
|
||
r"HDOP=(?P<hdop>[-+0-9.eE]+) VDOP=(?P<vdop>[-+0-9.eE]+)"
|
||
)
|
||
IMU_RE = re.compile(
|
||
r"(?P<t>[0-9]+\.[0-9]+) s \| samples=\s*(?P<n>[0-9]+) total=\s*(?P<total>[0-9]+) \| "
|
||
r"Temp=\s*(?P<temp>[-+0-9.]+) C \| Accel\[g\] "
|
||
r"X=\s*(?P<ax>[-+0-9.]+) Y=\s*(?P<ay>[-+0-9.]+) Z=\s*(?P<az>[-+0-9.]+) \| "
|
||
r"Gyro\[dps\] X=\s*(?P<gx>[-+0-9.]+) Y=\s*(?P<gy>[-+0-9.]+) Z=\s*(?P<gz>[-+0-9.]+)"
|
||
)
|
||
|
||
|
||
@dataclass
|
||
class GpsSample:
|
||
t: float
|
||
unix_s: float
|
||
lat: float
|
||
lon: float
|
||
height: float
|
||
hdop: float
|
||
vdop: float
|
||
|
||
|
||
@dataclass
|
||
class ImuSample:
|
||
t: float
|
||
unix_s: float
|
||
samples: int
|
||
temperature_c: float
|
||
accel_mps2: np.ndarray
|
||
gyro_rps: np.ndarray
|
||
|
||
|
||
def read_uart_csv(path: Path):
|
||
with path.open(newline="", encoding="utf-8") as stream:
|
||
yield from csv.DictReader(stream)
|
||
|
||
|
||
def parse_gps(path: Path) -> list[GpsSample]:
|
||
samples: list[GpsSample] = []
|
||
last_dop = (math.nan, math.nan)
|
||
for row in read_uart_csv(path):
|
||
line = row["uart_line"]
|
||
dop = DOP_RE.search(line)
|
||
if dop:
|
||
last_dop = (float(dop["hdop"]), float(dop["vdop"]))
|
||
match = GPS_RE.search(line)
|
||
if match:
|
||
samples.append(
|
||
GpsSample(
|
||
t=float(row["host_monotonic_s"]),
|
||
unix_s=float(row["host_unix_s"]),
|
||
lat=float(match["lat"]),
|
||
lon=float(match["lon"]),
|
||
height=float(match["height"]),
|
||
hdop=last_dop[0],
|
||
vdop=last_dop[1],
|
||
)
|
||
)
|
||
if len(samples) < 4:
|
||
raise ValueError(f"only {len(samples)} GPS positions found in {path}")
|
||
t0 = samples[0].t
|
||
for sample in samples:
|
||
sample.t -= t0
|
||
return samples
|
||
|
||
|
||
def parse_imu(path: Path) -> list[ImuSample]:
|
||
samples: list[ImuSample] = []
|
||
raw_meta: dict[str, float] | None = None
|
||
first_raw_tick: int | None = None
|
||
for row in read_uart_csv(path):
|
||
line = row["uart_line"]
|
||
|
||
if line.startswith("IMU_META,LSM6DSOX,"):
|
||
fields = line.split(",")[2:]
|
||
values: dict[str, float] = {}
|
||
for field in fields:
|
||
key, separator, value = field.partition("=")
|
||
if not separator:
|
||
continue
|
||
values[key] = float(value)
|
||
required = {
|
||
"tick_hz",
|
||
"odr_hz",
|
||
"accel_ug_lsb",
|
||
"gyro_udps_lsb",
|
||
"temp_lsb_c",
|
||
"temp_offset_millic",
|
||
}
|
||
missing = required.difference(values)
|
||
if missing:
|
||
raise ValueError(
|
||
f"LSM6DSOX metadata missing {sorted(missing)} in {path}"
|
||
)
|
||
if values["tick_hz"] <= 0.0 or values["temp_lsb_c"] <= 0.0:
|
||
raise ValueError(f"invalid LSM6DSOX metadata in {path}")
|
||
raw_meta = values
|
||
continue
|
||
|
||
if line.startswith("IMU_RAW,LSM6DSOX,"):
|
||
if raw_meta is None:
|
||
raise ValueError(f"LSM6DSOX raw sample precedes metadata in {path}")
|
||
fields = line.split(",")
|
||
if len(fields) != 11:
|
||
raise ValueError(f"malformed LSM6DSOX raw sample in {path}: {line}")
|
||
tick = int(fields[3])
|
||
if first_raw_tick is None:
|
||
first_raw_tick = tick
|
||
raw_accel = np.array([int(x) for x in fields[4:7]], dtype=float)
|
||
raw_gyro = np.array([int(x) for x in fields[7:10]], dtype=float)
|
||
raw_temperature = int(fields[10])
|
||
samples.append(
|
||
ImuSample(
|
||
t=(tick - first_raw_tick) / raw_meta["tick_hz"],
|
||
unix_s=float(row["host_unix_s"]),
|
||
samples=1,
|
||
temperature_c=(
|
||
raw_meta["temp_offset_millic"] / 1000.0
|
||
+ raw_temperature / raw_meta["temp_lsb_c"]
|
||
),
|
||
accel_mps2=(
|
||
G * raw_accel * raw_meta["accel_ug_lsb"] * 1e-6
|
||
),
|
||
gyro_rps=np.deg2rad(
|
||
raw_gyro * raw_meta["gyro_udps_lsb"] * 1e-6
|
||
),
|
||
)
|
||
)
|
||
continue
|
||
|
||
match = IMU_RE.search(line)
|
||
if not match:
|
||
continue
|
||
samples.append(
|
||
ImuSample(
|
||
t=float(match["t"]),
|
||
unix_s=float(row["host_unix_s"]),
|
||
samples=int(match["n"]),
|
||
temperature_c=float(match["temp"]),
|
||
accel_mps2=G
|
||
* np.array(
|
||
[float(match["ax"]), float(match["ay"]), float(match["az"])]
|
||
),
|
||
gyro_rps=np.deg2rad(
|
||
[float(match["gx"]), float(match["gy"]), float(match["gz"])]
|
||
),
|
||
)
|
||
)
|
||
if len(samples) < 4:
|
||
raise ValueError(f"only {len(samples)} IMU windows found in {path}")
|
||
if first_raw_tick is None:
|
||
t0 = samples[0].t
|
||
for sample in samples:
|
||
sample.t -= t0
|
||
return samples
|
||
|
||
|
||
def geodetic_to_ecef(lat_deg, lon_deg, height_m):
|
||
lat = np.deg2rad(np.asarray(lat_deg, dtype=float))
|
||
lon = np.deg2rad(np.asarray(lon_deg, dtype=float))
|
||
height = np.asarray(height_m, dtype=float)
|
||
a = 6378137.0
|
||
e2 = 6.69437999014e-3
|
||
n = a / np.sqrt(1.0 - e2 * np.sin(lat) ** 2)
|
||
x = (n + height) * np.cos(lat) * np.cos(lon)
|
||
y = (n + height) * np.cos(lat) * np.sin(lon)
|
||
z = (n * (1.0 - e2) + height) * np.sin(lat)
|
||
return np.column_stack((x, y, z))
|
||
|
||
|
||
def gps_to_enu(samples: list[GpsSample]):
|
||
lat0 = float(np.median([sample.lat for sample in samples]))
|
||
lon0 = float(np.median([sample.lon for sample in samples]))
|
||
h0 = float(np.median([sample.height for sample in samples]))
|
||
origin = geodetic_to_ecef([lat0], [lon0], [h0])[0]
|
||
xyz = geodetic_to_ecef(
|
||
[sample.lat for sample in samples],
|
||
[sample.lon for sample in samples],
|
||
[sample.height for sample in samples],
|
||
)
|
||
lat = math.radians(lat0)
|
||
lon = math.radians(lon0)
|
||
rotation = np.array(
|
||
[
|
||
[-math.sin(lon), math.cos(lon), 0.0],
|
||
[
|
||
-math.sin(lat) * math.cos(lon),
|
||
-math.sin(lat) * math.sin(lon),
|
||
math.cos(lat),
|
||
],
|
||
[
|
||
math.cos(lat) * math.cos(lon),
|
||
math.cos(lat) * math.sin(lon),
|
||
math.sin(lat),
|
||
],
|
||
]
|
||
)
|
||
return (xyz - origin) @ rotation.T, (lat0, lon0, h0)
|
||
|
||
|
||
def ecef_to_geodetic(xyz):
|
||
"""Convert ECEF XYZ to WGS-84 latitude, longitude and ellipsoid height."""
|
||
xyz = np.atleast_2d(np.asarray(xyz, dtype=float))
|
||
x, y, z = xyz.T
|
||
a = 6378137.0
|
||
e2 = 6.69437999014e-3
|
||
horizontal = np.hypot(x, y)
|
||
lon = np.arctan2(y, x)
|
||
lat = np.arctan2(z, horizontal * (1.0 - e2))
|
||
for _ in range(8):
|
||
n = a / np.sqrt(1.0 - e2 * np.sin(lat) ** 2)
|
||
height = horizontal / np.cos(lat) - n
|
||
lat = np.arctan2(z, horizontal * (1.0 - e2 * n / (n + height)))
|
||
n = a / np.sqrt(1.0 - e2 * np.sin(lat) ** 2)
|
||
height = horizontal / np.cos(lat) - n
|
||
return np.column_stack((np.rad2deg(lat), np.rad2deg(lon), height))
|
||
|
||
|
||
def enu_to_geodetic(enu, reference):
|
||
"""Convert local ENU positions back to WGS-84 coordinates."""
|
||
lat0, lon0, h0 = reference
|
||
lat = math.radians(lat0)
|
||
lon = math.radians(lon0)
|
||
rotation = np.array(
|
||
[
|
||
[-math.sin(lon), math.cos(lon), 0.0],
|
||
[
|
||
-math.sin(lat) * math.cos(lon),
|
||
-math.sin(lat) * math.sin(lon),
|
||
math.cos(lat),
|
||
],
|
||
[
|
||
math.cos(lat) * math.cos(lon),
|
||
math.cos(lat) * math.sin(lon),
|
||
math.sin(lat),
|
||
],
|
||
]
|
||
)
|
||
origin = geodetic_to_ecef([lat0], [lon0], [h0])[0]
|
||
xyz = origin + np.atleast_2d(np.asarray(enu, dtype=float)) @ rotation
|
||
return ecef_to_geodetic(xyz)
|
||
|
||
|
||
def skew(v):
|
||
x, y, z = v
|
||
return np.array([[0.0, -z, y], [z, 0.0, -x], [-y, x, 0.0]])
|
||
|
||
|
||
def quat_normalize(q):
|
||
q = np.asarray(q, dtype=float)
|
||
q /= np.linalg.norm(q)
|
||
return q if q[0] >= 0.0 else -q
|
||
|
||
|
||
def quat_multiply(q, r):
|
||
w, x, y, z = q
|
||
a, b, c, d = r
|
||
return np.array(
|
||
[
|
||
w * a - x * b - y * c - z * d,
|
||
w * b + x * a + y * d - z * c,
|
||
w * c - x * d + y * a + z * b,
|
||
w * d + x * c - y * b + z * a,
|
||
]
|
||
)
|
||
|
||
|
||
def rotvec_quat(v):
|
||
angle = float(np.linalg.norm(v))
|
||
if angle < 1e-10:
|
||
return quat_normalize(np.r_[1.0, 0.5 * np.asarray(v)])
|
||
return np.r_[math.cos(angle / 2.0), np.asarray(v) * math.sin(angle / 2.0) / angle]
|
||
|
||
|
||
def quat_rotation(q):
|
||
w, x, y, z = q
|
||
return np.array(
|
||
[
|
||
[1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)],
|
||
[2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)],
|
||
[2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)],
|
||
]
|
||
)
|
||
|
||
|
||
def align_quat(source, target):
|
||
source = np.asarray(source, dtype=float) / np.linalg.norm(source)
|
||
target = np.asarray(target, dtype=float) / np.linalg.norm(target)
|
||
dot = float(np.dot(source, target))
|
||
if dot < -0.999999:
|
||
axis = np.cross(source, [1.0, 0.0, 0.0])
|
||
if np.linalg.norm(axis) < 1e-6:
|
||
axis = np.cross(source, [0.0, 1.0, 0.0])
|
||
axis /= np.linalg.norm(axis)
|
||
return np.r_[0.0, axis]
|
||
return quat_normalize(np.r_[1.0 + dot, np.cross(source, target)])
|
||
|
||
|
||
def robust_sigma(values, floor):
|
||
values = np.asarray(values, dtype=float)
|
||
mad = np.median(np.abs(values - np.median(values)))
|
||
return max(float(1.4826 * mad), floor)
|
||
|
||
|
||
class StationaryEskf:
|
||
"""15-state ESKF with right attitude error in a local ENU frame."""
|
||
|
||
def __init__(self, position, attitude, accel_sigma, gyro_sigma, gps_sigma):
|
||
self.p = np.asarray(position, dtype=float).copy()
|
||
self.v = np.zeros(3)
|
||
self.q = quat_normalize(attitude)
|
||
self.ba = np.zeros(3)
|
||
self.bg = np.zeros(3)
|
||
self.p_cov = np.diag(
|
||
np.r_[gps_sigma**2, np.full(3, 1.0), np.deg2rad([5, 5, 30]) ** 2,
|
||
np.full(3, 0.05**2), np.full(3, np.deg2rad(0.05) ** 2)]
|
||
)
|
||
self.accel_sigma = np.asarray(accel_sigma)
|
||
self.gyro_sigma = np.asarray(gyro_sigma)
|
||
self.gps_r = np.diag(np.asarray(gps_sigma) ** 2)
|
||
self.rejected_gps = 0
|
||
self.accepted_gps = 0
|
||
|
||
def propagate(self, accel, gyro, dt):
|
||
if dt <= 0.0:
|
||
return
|
||
dt = min(float(dt), 1.0)
|
||
omega = np.asarray(gyro) - self.bg
|
||
specific_force = np.asarray(accel) - self.ba
|
||
rotation = quat_rotation(self.q)
|
||
acceleration = rotation @ specific_force + np.array([0.0, 0.0, -G])
|
||
self.p += self.v * dt + 0.5 * acceleration * dt * dt
|
||
self.v += acceleration * dt
|
||
self.q = quat_normalize(quat_multiply(self.q, rotvec_quat(omega * dt)))
|
||
|
||
f = np.zeros((15, 15))
|
||
f[0:3, 3:6] = np.eye(3)
|
||
f[3:6, 6:9] = -rotation @ skew(specific_force)
|
||
f[3:6, 9:12] = -rotation
|
||
f[6:9, 6:9] = -skew(omega)
|
||
f[6:9, 12:15] = -np.eye(3)
|
||
phi = np.eye(15) + f * dt
|
||
|
||
noise_map = np.zeros((15, 12))
|
||
noise_map[3:6, 0:3] = rotation
|
||
noise_map[6:9, 3:6] = np.eye(3)
|
||
noise_map[9:12, 6:9] = np.eye(3)
|
||
noise_map[12:15, 9:12] = np.eye(3)
|
||
qc = np.diag(
|
||
np.r_[
|
||
self.accel_sigma**2,
|
||
self.gyro_sigma**2,
|
||
np.full(3, 0.002**2),
|
||
np.full(3, np.deg2rad(0.002) ** 2),
|
||
]
|
||
)
|
||
self.p_cov = phi @ self.p_cov @ phi.T + noise_map @ qc @ noise_map.T * dt
|
||
self.p_cov = 0.5 * (self.p_cov + self.p_cov.T)
|
||
|
||
def _update(self, residual, h, r):
|
||
innovation = h @ self.p_cov @ h.T + r
|
||
gain = np.linalg.solve(innovation, h @ self.p_cov).T
|
||
delta = gain @ residual
|
||
identity = np.eye(15)
|
||
ikh = identity - gain @ h
|
||
self.p_cov = ikh @ self.p_cov @ ikh.T + gain @ r @ gain.T
|
||
self.p += delta[0:3]
|
||
self.v += delta[3:6]
|
||
self.q = quat_normalize(quat_multiply(self.q, rotvec_quat(delta[6:9])))
|
||
self.ba += delta[9:12]
|
||
self.bg += delta[12:15]
|
||
self.p_cov = 0.5 * (self.p_cov + self.p_cov.T)
|
||
return innovation
|
||
|
||
def gps_update(self, position, gate=16.27):
|
||
h = np.zeros((3, 15))
|
||
h[:, 0:3] = np.eye(3)
|
||
residual = np.asarray(position) - self.p
|
||
innovation = h @ self.p_cov @ h.T + self.gps_r
|
||
mahalanobis = float(residual @ np.linalg.solve(innovation, residual))
|
||
if mahalanobis > gate:
|
||
self.rejected_gps += 1
|
||
return False, mahalanobis
|
||
self._update(residual, h, self.gps_r)
|
||
self.accepted_gps += 1
|
||
return True, mahalanobis
|
||
|
||
def zero_velocity_update(self, sigma=0.025):
|
||
h = np.zeros((3, 15))
|
||
h[:, 3:6] = np.eye(3)
|
||
self._update(-self.v, h, np.eye(3) * sigma**2)
|
||
|
||
def zero_gyro_update(self, measured_gyro, sigma):
|
||
h = np.zeros((3, 15))
|
||
h[:, 12:15] = np.eye(3)
|
||
# z = bg + noise when the true angular rate is zero.
|
||
self._update(np.asarray(measured_gyro) - self.bg, h, np.diag(sigma**2))
|
||
|
||
|
||
def run_filter(gps: list[GpsSample], imu: list[ImuSample], enu):
|
||
accel = np.vstack([sample.accel_mps2 for sample in imu])
|
||
gyro = np.vstack([sample.gyro_rps for sample in imu])
|
||
mean_accel = np.mean(accel, axis=0)
|
||
attitude = align_quat(mean_accel, [0.0, 0.0, 1.0])
|
||
accel_sigma = np.array([robust_sigma(accel[:, i], 0.003) for i in range(3)])
|
||
gyro_sigma = np.array(
|
||
[robust_sigma(gyro[:, i], math.radians(0.001)) for i in range(3)]
|
||
)
|
||
gps_sigma = np.array([robust_sigma(enu[:, i], 1.0 if i < 2 else 2.0) for i in range(3)])
|
||
|
||
eskf = StationaryEskf(enu[0], attitude, accel_sigma, gyro_sigma, gps_sigma)
|
||
events = [(sample.t, 0, i) for i, sample in enumerate(imu)]
|
||
events += [(sample.t, 1, i) for i, sample in enumerate(gps)]
|
||
events.sort()
|
||
current_accel = accel[0]
|
||
current_gyro = gyro[0]
|
||
last_t = 0.0
|
||
output = []
|
||
|
||
for t, kind, index in events:
|
||
eskf.propagate(current_accel, current_gyro, t - last_t)
|
||
last_t = t
|
||
if kind == 0:
|
||
current_accel = accel[index]
|
||
current_gyro = gyro[index]
|
||
eskf.zero_velocity_update()
|
||
eskf.zero_gyro_update(current_gyro, np.maximum(gyro_sigma, math.radians(0.002)))
|
||
else:
|
||
accepted, mahalanobis = eskf.gps_update(enu[index])
|
||
output.append(
|
||
{
|
||
"t": t,
|
||
"raw": enu[index].copy(),
|
||
"fused": eskf.p.copy(),
|
||
"velocity": eskf.v.copy(),
|
||
"position_std": np.sqrt(np.maximum(np.diag(eskf.p_cov)[0:3], 0.0)),
|
||
"accepted": accepted,
|
||
"mahalanobis": mahalanobis,
|
||
}
|
||
)
|
||
return eskf, output, accel_sigma, gyro_sigma, gps_sigma, attitude
|
||
|
||
|
||
def radial_95(values):
|
||
centered = values - np.median(values, axis=0)
|
||
return float(np.percentile(np.linalg.norm(centered, axis=1), 95))
|
||
|
||
|
||
def write_csvs(output_dir, gps, imu, enu, filtered):
|
||
with (output_dir / "gps_positions.csv").open("w", newline="") as stream:
|
||
writer = csv.writer(stream)
|
||
writer.writerow(["t_s", "unix_s", "lat_deg", "lon_deg", "height_m", "east_m", "north_m", "up_m", "hdop", "vdop"])
|
||
for sample, local in zip(gps, enu):
|
||
writer.writerow([sample.t, sample.unix_s, sample.lat, sample.lon, sample.height, *local, sample.hdop, sample.vdop])
|
||
with (output_dir / "imu_windows.csv").open("w", newline="") as stream:
|
||
writer = csv.writer(stream)
|
||
writer.writerow(["t_s", "unix_s", "samples", "temperature_c", "ax_mps2", "ay_mps2", "az_mps2", "gx_rps", "gy_rps", "gz_rps"])
|
||
for sample in imu:
|
||
writer.writerow([sample.t, sample.unix_s, sample.samples, sample.temperature_c, *sample.accel_mps2, *sample.gyro_rps])
|
||
with (output_dir / "eskf_positions.csv").open("w", newline="") as stream:
|
||
writer = csv.writer(stream)
|
||
writer.writerow(["t_s", "gps_e_m", "gps_n_m", "gps_u_m", "eskf_e_m", "eskf_n_m", "eskf_u_m", "ve_mps", "vn_mps", "vu_mps", "sigma_e_m", "sigma_n_m", "sigma_u_m", "gps_accepted", "mahalanobis2"])
|
||
for row in filtered:
|
||
writer.writerow([row["t"], *row["raw"], *row["fused"], *row["velocity"], *row["position_std"], int(row["accepted"]), row["mahalanobis"]])
|
||
|
||
|
||
def write_playback_map(output_dir, gps, filtered, reference):
|
||
"""Write a Leaflet map with a sample-by-sample GPS/ESKF player."""
|
||
fused_lla = enu_to_geodetic(
|
||
np.vstack([row["fused"] for row in filtered]), reference
|
||
)
|
||
records = []
|
||
for index, (sample, row, corrected) in enumerate(zip(gps, filtered, fused_lla)):
|
||
records.append(
|
||
{
|
||
"i": index,
|
||
"t": round(float(row["t"]), 6),
|
||
"raw": [sample.lat, sample.lon, sample.height],
|
||
"fused": [float(corrected[0]), float(corrected[1]), float(corrected[2])],
|
||
"accepted": bool(row["accepted"]),
|
||
"mahalanobis": round(float(row["mahalanobis"]), 3),
|
||
"separation": round(float(np.linalg.norm(row["raw"][:2] - row["fused"][:2])), 3),
|
||
}
|
||
)
|
||
|
||
data_json = json.dumps(records, separators=(",", ":"), ensure_ascii=False)
|
||
center_json = json.dumps([reference[0], reference[1]])
|
||
html = """<!doctype html>
|
||
<html lang="tr">
|
||
<head>
|
||
<meta charset="utf-8">
|
||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||
<title>GPS / ESKF sample oynatıcı</title>
|
||
<link rel="stylesheet" href="https://unpkg.com/leaflet@1.9.4/dist/leaflet.css">
|
||
<style>
|
||
html,body,#map { height:100%; margin:0; font-family:system-ui,sans-serif; }
|
||
.panel { position:absolute; z-index:1000; left:12px; bottom:18px; width:min(620px,calc(100% - 44px));
|
||
background:rgba(255,255,255,.94); padding:12px 14px; border-radius:9px; box-shadow:0 2px 12px #0004; }
|
||
.row { display:flex; align-items:center; gap:9px; flex-wrap:wrap; }
|
||
button,select { padding:6px 10px; } input[type=range] { flex:1; min-width:180px; }
|
||
.readout { margin-top:8px; display:grid; grid-template-columns:repeat(3,minmax(120px,1fr)); gap:4px 12px; font-size:13px; }
|
||
.raw { color:#d62728; } .fused { color:#1769aa; } .gated { color:#e67e22; font-weight:700; }
|
||
.legend { background:white; padding:7px 9px; line-height:1.5; box-shadow:0 1px 5px #0004; }
|
||
.dot { display:inline-block; width:10px; height:10px; border-radius:50%; margin-right:5px; }
|
||
@media(max-width:600px) { .readout { grid-template-columns:1fr 1fr; } }
|
||
</style>
|
||
</head>
|
||
<body>
|
||
<div id="map"></div>
|
||
<div class="panel">
|
||
<div class="row">
|
||
<button id="play">▶ Oynat</button><button id="reset">↺ Başa al</button><button id="fit">İzleri sığdır</button>
|
||
<label>Hız <select id="speed"><option>0.5</option><option selected>1</option><option>2</option><option>4</option></select>×</label>
|
||
</div>
|
||
<div class="row" style="margin-top:7px"><span id="tick">1 / 1</span><input id="slider" type="range" min="0" value="0"><b id="time">0.0 s</b></div>
|
||
<div class="readout">
|
||
<span class="raw" id="raw"></span><span class="fused" id="fused"></span><span id="height"></span>
|
||
<span id="distance"></span><span id="gate"></span><span id="nis"></span>
|
||
</div>
|
||
</div>
|
||
<script src="https://unpkg.com/leaflet@1.9.4/dist/leaflet.js"></script>
|
||
<script>
|
||
const samples=__DATA__, center=__CENTER__;
|
||
const map=L.map('map',{zoomControl:true}).setView(center,20);
|
||
L.tileLayer('https://tile.openstreetmap.org/{z}/{x}/{y}.png',{maxZoom:22,attribution:'© OpenStreetMap contributors'}).addTo(map);
|
||
L.control.scale({imperial:false}).addTo(map);
|
||
const rawLine=L.polyline([],{color:'#d62728',weight:2,opacity:.55}).addTo(map);
|
||
const fusedLine=L.polyline([],{color:'#1769aa',weight:3,opacity:.9}).addTo(map);
|
||
const rawDots=L.layerGroup().addTo(map), fusedDots=L.layerGroup().addTo(map);
|
||
const rawNow=L.circleMarker(center,{radius:8,color:'#8b0000',weight:2,fillColor:'#d62728',fillOpacity:.9}).addTo(map);
|
||
const fusedNow=L.circleMarker(center,{radius:7,color:'#083b66',weight:2,fillColor:'#2196f3',fillOpacity:.95}).addTo(map);
|
||
const legend=L.control({position:'topright'}); legend.onAdd=()=>{const d=L.DomUtil.create('div','legend');d.innerHTML='<b>GPS / IMU ESKF</b><br><span class="dot" style="background:#d62728"></span>Ham GPS<br><span class="dot" style="background:#2196f3"></span>ESKF + ZUPT<br><span class="dot" style="background:#e67e22"></span>Reddedilen GPS';return d};legend.addTo(map);
|
||
const slider=document.getElementById('slider'); slider.max=samples.length-1;
|
||
let index=0,timer=null;
|
||
function fmt(v,n=7){return Number(v).toFixed(n)}
|
||
function render(i){
|
||
index=Math.max(0,Math.min(samples.length-1,Number(i))); slider.value=index;
|
||
const shown=samples.slice(0,index+1), s=samples[index];
|
||
rawLine.setLatLngs(shown.map(x=>x.raw.slice(0,2))); fusedLine.setLatLngs(shown.map(x=>x.fused.slice(0,2)));
|
||
rawDots.clearLayers(); fusedDots.clearLayers();
|
||
shown.forEach(x=>{
|
||
L.circleMarker(x.raw.slice(0,2),{radius:2,stroke:false,fillColor:x.accepted?'#d62728':'#e67e22',fillOpacity:.7}).addTo(rawDots);
|
||
L.circleMarker(x.fused.slice(0,2),{radius:2,stroke:false,fillColor:'#1769aa',fillOpacity:.8}).addTo(fusedDots);
|
||
});
|
||
rawNow.setLatLng(s.raw.slice(0,2)).setStyle({fillColor:s.accepted?'#d62728':'#e67e22'});
|
||
fusedNow.setLatLng(s.fused.slice(0,2));
|
||
document.getElementById('tick').textContent=`${index+1} / ${samples.length}`;
|
||
document.getElementById('time').textContent=`${s.t.toFixed(3)} s`;
|
||
document.getElementById('raw').textContent=`GPS: ${fmt(s.raw[0])}, ${fmt(s.raw[1])}`;
|
||
document.getElementById('fused').textContent=`ESKF: ${fmt(s.fused[0])}, ${fmt(s.fused[1])}`;
|
||
document.getElementById('height').textContent=`h: ${s.raw[2].toFixed(1)} → ${s.fused[2].toFixed(1)} m`;
|
||
document.getElementById('distance').textContent=`yatay fark: ${s.separation.toFixed(2)} m`;
|
||
const gate=document.getElementById('gate'); gate.textContent=s.accepted?'GPS kabul':'GPS RED'; gate.className=s.accepted?'':'gated';
|
||
document.getElementById('nis').textContent=`NIS: ${s.mahalanobis.toFixed(2)}`;
|
||
}
|
||
function stop(){if(timer){clearTimeout(timer);timer=null}document.getElementById('play').textContent='▶ Oynat'}
|
||
function step(){
|
||
if(index>=samples.length-1){stop();return}
|
||
const dt=Math.max(60,Math.min(2000,(samples[index+1].t-samples[index].t)*1000/Number(document.getElementById('speed').value)));
|
||
timer=setTimeout(()=>{render(index+1);step()},dt);
|
||
}
|
||
document.getElementById('play').onclick=()=>{if(timer){stop()}else{if(index>=samples.length-1)render(0);document.getElementById('play').textContent='⏸ Duraklat';step()}};
|
||
document.getElementById('reset').onclick=()=>{stop();render(0)};
|
||
document.getElementById('fit').onclick=()=>map.fitBounds(L.latLngBounds(samples.flatMap(s=>[s.raw.slice(0,2),s.fused.slice(0,2)])).pad(.15));
|
||
slider.oninput=()=>{stop();render(slider.value)};
|
||
render(0); map.fitBounds(L.latLngBounds(samples.flatMap(s=>[s.raw.slice(0,2),s.fused.slice(0,2)])).pad(.15));
|
||
</script>
|
||
</body></html>
|
||
""".replace("__DATA__", data_json).replace("__CENTER__", center_json)
|
||
(output_dir / "trajectory_map.html").write_text(html, encoding="utf-8")
|
||
|
||
|
||
def plot_results(output_dir, gps, imu, filtered):
|
||
time = np.array([row["t"] for row in filtered])
|
||
raw = np.vstack([row["raw"] for row in filtered])
|
||
fused = np.vstack([row["fused"] for row in filtered])
|
||
labels = ["East", "North", "Up"]
|
||
fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
|
||
for i, axis in enumerate(axes):
|
||
axis.plot(time, raw[:, i], ".", color="0.65", ms=3, label="GPS WLS")
|
||
axis.plot(time, fused[:, i], color="tab:blue", lw=1.8, label="ESKF + ZUPT")
|
||
axis.set_ylabel(f"{labels[i]} [m]")
|
||
axis.grid(alpha=0.25)
|
||
axes[0].legend()
|
||
axes[-1].set_xlabel("Normalized elapsed time [s]")
|
||
fig.suptitle("Stationary GPS / IMU error-state Kalman filter")
|
||
fig.tight_layout()
|
||
fig.savefig(output_dir / "position_timeseries.png", dpi=160)
|
||
plt.close(fig)
|
||
|
||
accepted = np.array([row["accepted"] for row in filtered])
|
||
fig, axis = plt.subplots(figsize=(7, 7))
|
||
axis.scatter(raw[accepted, 0], raw[accepted, 1], s=12, alpha=0.5, color="0.5", label="GPS WLS accepted")
|
||
axis.scatter(raw[~accepted, 0], raw[~accepted, 1], marker="x", s=55, color="tab:red", label="GPS gated")
|
||
axis.plot(fused[:, 0], fused[:, 1], color="tab:blue", lw=1.5, label="ESKF + ZUPT")
|
||
axis.scatter(fused[-1, 0], fused[-1, 1], marker="x", s=70, color="tab:red", label="ESKF final")
|
||
axis.set_aspect("equal", adjustable="box")
|
||
axis.set_xlabel("East [m]")
|
||
axis.set_ylabel("North [m]")
|
||
axis.grid(alpha=0.25)
|
||
axis.legend()
|
||
center = np.median(raw[:, :2], axis=0)
|
||
central_radius = max(20.0, 1.2 * float(np.percentile(np.abs(raw[:, :2] - center), 99)))
|
||
axis.set_xlim(center[0] - central_radius, center[0] + central_radius)
|
||
axis.set_ylim(center[1] - central_radius, center[1] + central_radius)
|
||
axis.set_title(f"Horizontal stationary scatter (central view; gated={np.sum(~accepted)})")
|
||
fig.tight_layout()
|
||
fig.savefig(output_dir / "horizontal_scatter.png", dpi=160)
|
||
plt.close(fig)
|
||
|
||
t_imu = np.array([sample.t for sample in imu])
|
||
accel = np.vstack([sample.accel_mps2 for sample in imu]) / G
|
||
gyro = np.rad2deg(np.vstack([sample.gyro_rps for sample in imu]))
|
||
fig, axes = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
|
||
for i, name in enumerate("XYZ"):
|
||
axes[0].plot(t_imu, accel[:, i], label=name)
|
||
axes[1].plot(t_imu, gyro[:, i], label=name)
|
||
axes[0].set_ylabel("Acceleration [g]")
|
||
axes[1].set_ylabel("Angular rate [deg/s]")
|
||
axes[1].set_xlabel("IMU elapsed time [s]")
|
||
for axis in axes:
|
||
axis.grid(alpha=0.25)
|
||
axis.legend(ncol=3)
|
||
fig.suptitle("ICM-4268x 500 ms window averages")
|
||
fig.tight_layout()
|
||
fig.savefig(output_dir / "imu_stationary.png", dpi=160)
|
||
plt.close(fig)
|
||
|
||
|
||
def main():
|
||
parser = argparse.ArgumentParser()
|
||
parser.add_argument("--gps", type=Path, default=Path("logs/gps_60s.csv"))
|
||
parser.add_argument("--imu", type=Path, default=Path("logs/imu_60s.csv"))
|
||
parser.add_argument("--output", type=Path, default=Path("output"))
|
||
args = parser.parse_args()
|
||
args.output.mkdir(parents=True, exist_ok=True)
|
||
|
||
gps = parse_gps(args.gps)
|
||
imu = parse_imu(args.imu)
|
||
enu, reference = gps_to_enu(gps)
|
||
eskf, filtered, accel_sigma, gyro_sigma, gps_sigma, attitude = run_filter(gps, imu, enu)
|
||
write_csvs(args.output, gps, imu, enu, filtered)
|
||
plot_results(args.output, gps, imu, filtered)
|
||
write_playback_map(args.output, gps, filtered, reference)
|
||
|
||
raw = np.vstack([row["raw"] for row in filtered])
|
||
fused = np.vstack([row["fused"] for row in filtered])
|
||
raw_std = np.std(raw, axis=0, ddof=1)
|
||
fused_std = np.std(fused, axis=0, ddof=1)
|
||
raw_h95 = radial_95(raw[:, :2])
|
||
fused_h95 = radial_95(fused[:, :2])
|
||
imu_accel = np.vstack([sample.accel_mps2 for sample in imu])
|
||
imu_gyro = np.vstack([sample.gyro_rps for sample in imu])
|
||
mean_accel = np.mean(imu_accel, axis=0)
|
||
mean_gyro = np.mean(imu_gyro, axis=0)
|
||
lines = [
|
||
"Stationary GPS/IMU ESKF result",
|
||
"================================",
|
||
f"GPS positions: {len(gps)} over {gps[-1].t - gps[0].t:.3f} s",
|
||
f"IMU windows: {len(imu)} over {imu[-1].t - imu[0].t:.3f} s",
|
||
f"Underlying SPI samples: {sum(sample.samples for sample in imu)}",
|
||
"Capture mode: sequential logs replayed on a common elapsed-time axis",
|
||
f"ENU reference (capture median): lat={reference[0]:.10f} deg, lon={reference[1]:.10f} deg, h={reference[2]:.3f} m",
|
||
f"GPS robust sigma E/N/U [m]: {gps_sigma[0]:.3f}, {gps_sigma[1]:.3f}, {gps_sigma[2]:.3f}",
|
||
f"GPS accepted/rejected: {eskf.accepted_gps}/{eskf.rejected_gps}",
|
||
f"Raw GPS std E/N/U [m]: {raw_std[0]:.3f}, {raw_std[1]:.3f}, {raw_std[2]:.3f}",
|
||
f"ESKF std E/N/U [m]: {fused_std[0]:.3f}, {fused_std[1]:.3f}, {fused_std[2]:.3f}",
|
||
f"Raw/ESKF horizontal radial 95% [m]: {raw_h95:.3f} / {fused_h95:.3f}",
|
||
f"IMU mean acceleration XYZ [m/s^2]: {mean_accel[0]:.6f}, {mean_accel[1]:.6f}, {mean_accel[2]:.6f}",
|
||
f"IMU mean angular rate XYZ [deg/s]: {np.rad2deg(mean_gyro)[0]:.6f}, {np.rad2deg(mean_gyro)[1]:.6f}, {np.rad2deg(mean_gyro)[2]:.6f}",
|
||
f"IMU robust accel noise XYZ [m/s^2]: {accel_sigma[0]:.6f}, {accel_sigma[1]:.6f}, {accel_sigma[2]:.6f}",
|
||
f"IMU robust gyro noise XYZ [deg/s]: {np.rad2deg(gyro_sigma)[0]:.6f}, {np.rad2deg(gyro_sigma)[1]:.6f}, {np.rad2deg(gyro_sigma)[2]:.6f}",
|
||
f"Initial body-to-ENU quaternion [w,x,y,z]: {attitude[0]:.8f}, {attitude[1]:.8f}, {attitude[2]:.8f}, {attitude[3]:.8f}",
|
||
f"Final ESKF position E/N/U [m]: {eskf.p[0]:.3f}, {eskf.p[1]:.3f}, {eskf.p[2]:.3f}",
|
||
f"Final ESKF velocity E/N/U [m/s]: {eskf.v[0]:.6f}, {eskf.v[1]:.6f}, {eskf.v[2]:.6f}",
|
||
"Caution: no surveyed ground truth; reported gains are precision/stability, not absolute accuracy.",
|
||
"Caution: ZUPT is enabled because the board was declared stationary.",
|
||
]
|
||
summary = "\n".join(lines) + "\n"
|
||
(args.output / "summary.txt").write_text(summary, encoding="utf-8")
|
||
print(summary, end="")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|