""" allan_gyro.py: simulate a gyroscope at rest and read its noise off an Allan deviation plot. EECE 5554 Robotics Sensing and Navigation, demo for Lab 3. A gyroscope sitting still should read zero. It doesn't. Three noise terms matter most, and each one has its own slope on a log-log Allan deviation plot: angle random walk (ARW), N white rate noise slope -1/2 read at tau = 1 s bias instability, B flicker (1/f) noise flat flat part = 0.664 B rate random walk (RRW), K random walk of rate slope +1/2 read at tau = 3 s This follows IEEE Std 952 and the Lab 3 procedure: record the IMU at rest, compute the overlapping Allan deviation, and read N, B, and K off the plot. Units used here (the ones on IMU datasheets): N in deg/sqrt(hr), B in deg/hr, K in deg/hr/sqrt(hr), Allan deviation in deg/hr. Run it from a terminal: python3 allan_gyro.py default MEMS-like gyro python3 allan_gyro.py 0.3 10 0 N, B, K """ import json import math import sys import numpy as np DEG = math.pi / 180.0 BIAS_FACTOR = math.sqrt(2 * math.log(2) / math.pi) # 0.664: flat Allan deviation = 0.664 B def simulate_gyro(arw=0.21, bias_inst=8.0, rrw=0.0, fs=40.0, hours=1.0, seed=3): """Gyro rate at rest, in rad/s, sampled at fs Hz for `hours`.""" rng = np.random.default_rng(seed) n = int(fs * hours * 3600) n_si = arw * DEG / 60.0 # rad/s/sqrt(Hz) (deg/sqrt(hr) -> rad/sqrt(s)) b_si = bias_inst * DEG / 3600.0 # rad/s k_si = rrw * DEG / 3600.0 / 60.0 # rad/s/sqrt(s) rate = np.zeros(n) # 1) white rate noise: per-sample std N * sqrt(fs) gives Allan deviation N / sqrt(tau) rate += rng.normal(0.0, n_si * math.sqrt(fs), n) # 2) flicker noise: shape white noise to the IEEE 952 bias-instability PSD, # B^2 / (2 pi f) two-sided, which is B^2 / (pi f) one-sided if b_si > 0: w = np.fft.rfft(rng.normal(0.0, 1.0, n)) f = np.fft.rfftfreq(n, 1.0 / fs) shape = np.zeros_like(f) # unit white noise has a one-sided PSD of 2 / fs, so scale by sqrt(target / (2 / fs)) shape[1:] = np.sqrt((b_si ** 2 / (math.pi * f[1:])) * fs / 2.0) rate += np.fft.irfft(w * shape, n) # 3) rate random walk: the bias itself wanders, b[k+1] = b[k] + K sqrt(dt) w if k_si > 0: rate += np.cumsum(rng.normal(0.0, k_si * math.sqrt(1.0 / fs), n)) return rate def allan_deviation(rate, fs, points=60): """Overlapping Allan deviation at log-spaced cluster times. Returns tau (s) and adev (same units as rate).""" n = len(rate) theta = np.concatenate([[0.0], np.cumsum(rate) / fs]) # integrated angle m_max = max(1, (n - 1) // 9) ms = np.unique(np.logspace(0, math.log10(m_max), points).astype(int)) taus, adev = [], [] for m in ms: tau = m / fs d = theta[2 * m:] - 2 * theta[m:-m] + theta[:-2 * m] avar = (d ** 2).sum() / (2 * tau ** 2 * (n + 1 - 2 * m)) taus.append(tau) adev.append(math.sqrt(avar)) return np.array(taus), np.array(adev) def read_noise(taus, adev_dph): """Read N, B, K off the curve the way the lab asks you to.""" lt, la = np.log10(taus), np.log10(adev_dph) at1 = 10 ** np.interp(0.0, lt, la) # deg/hr at tau = 1 s i_min = int(np.argmin(adev_dph)) out = {"N_read": float(at1 / 60.0), # deg/hr * sqrt(s) -> deg/sqrt(hr) "B_read": float(adev_dph[i_min] / BIAS_FACTOR), "tau_min": float(taus[i_min])} # If the curve climbs well above its bottom on the right, a +1/2 tail is there. # Remove the flat bottom level, put the rest on the line K sqrt(tau / 3), and take the # median over those tail points. This is rough: the tail is the noisiest part of the plot. floor2 = adev_dph[i_min] ** 2 ks = [math.sqrt(a * a - floor2) * math.sqrt(3.0 / t) for t, a in zip(taus[i_min:], adev_dph[i_min:]) if a * a > 3 * floor2] if len(ks) >= 2: out["K_read"] = float(np.median(ks) * 60.0) # deg/hr at tau = 3 s, times 60 -> deg/hr/sqrt(hr) return out def demo_api(params_json): """Web page entry point. Input: {"arw":..,"bias":..,"rrw":..,"fs":..,"hours":..,"seed":..}.""" p = json.loads(params_json or "{}") arw, b, k = float(p.get("arw", 0.21)), float(p.get("bias", 8.0)), float(p.get("rrw", 0.0)) fs, hours = float(p.get("fs", 40.0)), float(p.get("hours", 1.0)) rate = simulate_gyro(arw, b, k, fs, hours, int(p.get("seed", 3))) taus, adev = allan_deviation(rate, fs) adev_dph = adev / DEG * 3600.0 out = {"taus": taus.tolist(), "adev": adev_dph.tolist(), "fs": fs, "hours": hours, "set": {"N": arw, "B": b, "K": k}} out.update(read_noise(taus, adev_dph)) # model lines, deg/hr: N / sqrt(tau), 0.664 B, K sqrt(tau / 3) out["lines"] = { "arw": [(arw * 60.0) / math.sqrt(t) for t in taus], "bias": [BIAS_FACTOR * b for _ in taus], "rrw": [(k / 60.0) * math.sqrt(t / 3.0) for t in taus], } # the first 60 s of raw data, in deg/s, for a quick look m = int(min(len(rate), 60 * fs)) step = max(1, m // 600) out["raw"] = {"t": (np.arange(0, m, step) / fs).round(3).tolist(), "w": (rate[:m:step] / DEG).round(5).tolist()} return json.dumps(out) def main(argv): arw = float(argv[1]) if len(argv) > 1 else 0.21 b = float(argv[2]) if len(argv) > 2 else 8.0 k = float(argv[3]) if len(argv) > 3 else 0.0 r = json.loads(demo_api(json.dumps({"arw": arw, "bias": b, "rrw": k}))) print("set: N = {:.3f} deg/sqrt(hr) B = {:.2f} deg/hr K = {:.2f} deg/hr/sqrt(hr)".format(arw, b, k)) print("read: N = {:.3f} deg/sqrt(hr) B = {:.2f} deg/hr (minimum at tau = {:.0f} s)".format( r["N_read"], r["B_read"], r["tau_min"]), " K = {:.2f}".format(r["K_read"]) if "K_read" in r else "") try: import matplotlib.pyplot as plt plt.loglog(r["taus"], r["adev"], label="Allan deviation") plt.loglog(r["taus"], r["lines"]["arw"], "--", label="N / sqrt(tau)") plt.loglog(r["taus"], r["lines"]["bias"], "--", label="0.664 B") plt.xlabel("tau (s)"); plt.ylabel("Allan deviation (deg/hr)"); plt.grid(True, which="both"); plt.legend() plt.show() except ImportError: pass if __name__ == "__main__": main(sys.argv)