#!/usr/bin/env python3 # Reference implementation — Trip Analyst Forge expedition (engineermaxxing.com). # Build it yourself from the milestone steps first; use this only to get unstuck. # Golden artifacts on the site were produced by THIS file (numpy==2.2.6, seed 42). """FINAL golden pipeline — Trip Analyst Forge expedition (comma2k19 Example_1 seg 40). Challenge feed: GNSS decimated to 1 Hz + seeded N(0, 3m) noise (seed 42) — disclosed in the lesson; clean 10 Hz feed kept as reference. numpy-only, deterministic.""" import json, os import numpy as np SEG, OUT = "seg", "artifacts" os.makedirs(OUT, exist_ok=True) META = {"dataset": "comma2k19 Example_1 b0c9d2329ad1606b|2018-08-02--08-34-47 seg 40", "challenge_feed": "gnss @1Hz + N(0,3m) seed42", "pinned_at": "2026-07-28"} def L(p): return np.load(os.path.join(SEG, p)) gt_t = L("global_pose/frame_times"); gt_ecef = L("global_pose/frame_positions") gt_vel = L("global_pose/frame_velocities") can_st = L("processed_log/CAN/speed/t"); can_sv = L("processed_log/CAN/speed/value")[:,0] str_t = L("processed_log/CAN/steering_angle/t"); str_v = L("processed_log/CAN/steering_angle/value") imu_t = L("processed_log/IMU/gyro/t"); gyro = L("processed_log/IMU/gyro/value") gns_t = L("processed_log/GNSS/live_gnss_ublox/t"); gns = L("processed_log/GNSS/live_gnss_ublox/value") t0 = gt_t[0] gt_t, can_st, str_t, imu_t, gns_t = [a-t0 for a in (gt_t, can_st, str_t, imu_t, gns_t)] A_E, E2 = 6378137.0, 6.69437999014e-3 def geo2ecef(lat, lon, alt): la, lo = np.radians(lat), np.radians(lon) N = A_E/np.sqrt(1-E2*np.sin(la)**2) return np.stack([(N+alt)*np.cos(la)*np.cos(lo), (N+alt)*np.cos(la)*np.sin(lo), (N*(1-E2)+alt)*np.sin(la)], -1) ref_lat, ref_lon, ref_alt = gns[0,0], gns[0,1], gns[0,4] ref_ecef = geo2ecef(ref_lat, ref_lon, ref_alt) la, lo = np.radians(ref_lat), np.radians(ref_lon) R_ENU = np.array([[-np.sin(lo), np.cos(lo), 0], [-np.sin(la)*np.cos(lo), -np.sin(la)*np.sin(lo), np.cos(la)], [ np.cos(la)*np.cos(lo), np.cos(la)*np.sin(lo), np.sin(la)]]) def ecef2enu(p): return (R_ENU @ (p-ref_ecef).T).T gt_enu = ecef2enu(gt_ecef); gt_venu = (R_ENU @ gt_vel.T).T gns_enu = ecef2enu(geo2ecef(gns[:,0], gns[:,1], gns[:,4])) def interp_to(tq, ts, vs): if vs.ndim == 1: return np.interp(tq, ts, vs) return np.stack([np.interp(tq, ts, vs[:,i]) for i in range(vs.shape[1])], -1) def rmse(a, b): return float(np.sqrt(np.mean(np.sum((a-b)**2, axis=1)))) tg = gns_t # ~9.7 Hz working grid (579) gt10 = interp_to(tg, gt_t, gt_enu[:, :2]) spd10= interp_to(tg, can_st, can_sv) gy10 = interp_to(tg, imu_t, gyro[:,2]) gt_v = interp_to(tg, gt_t, gt_venu[:, :2]); gt_hdg = np.arctan2(gt_v[:,1], gt_v[:,0]) # challenge feed rng = np.random.default_rng(42) mask = np.zeros(len(gns_t), bool); last=-10 for i,t in enumerate(gns_t): if t-last >= 0.999: mask[i]=True; last=t ch_t = gns_t[mask] ch_z = gns_enu[mask, :2] + rng.normal(0, 3.0, (int(mask.sum()), 2)) gt_ch = interp_to(ch_t, gt_t, gt_enu[:, :2]) CHI2 = 5.991 # 95%, 2 dof R_CH = 9.0 # ---- M1 ---- dist = float(np.sum(np.linalg.norm(np.diff(gt_enu[:, :2], axis=0), axis=1))) m1 = {"schema":"forge.stats.v1","milestone":"m1", **META, "duration_s": round(float(gt_t[-1]),2), "n_gt_frames": int(len(gt_t)), "rate_hz": {"gt":20.0, "gnss_clean": round(len(gns_t)/gns_t[-1],1), "gnss_challenge": round(len(ch_t)/ch_t[-1],1), "can_speed": round(len(can_st)/can_st[-1],1), "imu": round(len(imu_t)/imu_t[-1],1)}, "distance_m": round(dist,1), "speed_ms": {"min": round(float(can_sv.min()),2), "max": round(float(can_sv.max()),2)}, "clean_gnss_rmse_m": round(rmse(gns_enu[:, :2], gt10),3), "challenge_gnss_rmse_m": round(rmse(ch_z, gt_ch),3)} # ---- M2: CV KF on challenge feed ---- def kf_cv(z, ts, q, r): x = np.array([z[0,0], z[0,1], 0, 0], float); P = np.diag([r,r,100.,100.]) H = np.array([[1.,0,0,0],[0,1.,0,0]]); Rm = np.eye(2)*r xs = np.zeros((len(z),4)); nis = np.zeros(len(z)) for k in range(len(z)): if k: dt = ts[k]-ts[k-1]; F = np.eye(4); F[0,2]=F[1,3]=dt G = np.array([[dt*dt/2,0],[0,dt*dt/2],[dt,0],[0,dt]]) x = F@x; P = F@P@F.T + q*(G@G.T) y = z[k]-H@x; S = H@P@H.T+Rm nis[k] = float(y@np.linalg.solve(S,y)) K = P@H.T@np.linalg.inv(S); x = x+K@y; P = (np.eye(4)-K@H)@P xs[k] = x return xs, nis sweep = {} for q in [0.05,0.2,0.5,1.0,2.0,5.0]: xs,nis = sweep[q] = kf_cv(ch_z, ch_t, q, R_CH) sweep[q] = (rmse(xs[:, :2], gt_ch), float(np.mean(nis<=CHI2)*100)) q_star = min(sweep, key=lambda q: sweep[q][0]) kf_rmse, kf_nis = sweep[q_star] xs_kf,_ = kf_cv(ch_z, ch_t, q_star, R_CH) m2 = {"schema":"forge.state.v1","milestone":"m2","method":"kf_cv","feed":"challenge", "q": q_star, "r_m": 3.0, "rmse_m": round(kf_rmse,3), "raw_feed_rmse_m": m1["challenge_gnss_rmse_m"], "nis_inside_95_pct": round(kf_nis,1), "sweep": {str(q): round(v[0],3) for q,v in sweep.items()}, "broken": {"q_tiny_rmse_m": round(sweep[0.05][0],3)}} # ---- M3: EKF unicycle dead-reckon @10Hz, corrections @1Hz ---- def ekf(qpos,qth,qv,gybias=0.0): th0 = np.arctan2(gt_enu[20,1]-gt_enu[0,1], gt_enu[20,0]-gt_enu[0,0]) x = np.array([ch_z[0,0],ch_z[0,1],th0,spd10[0]]); P = np.diag([R_CH,R_CH,0.3,4.]) H = np.array([[1.,0,0,0],[0,1.,0,0]]); Rm = np.eye(2)*R_CH xs = np.zeros((len(tg),4)); nees = np.zeros(len(tg)) for k in range(len(tg)): if k: dt = tg[k]-tg[k-1]; th,v = x[2],x[3] x = np.array([x[0]+v*np.cos(th)*dt, x[1]+v*np.sin(th)*dt, th+(gy10[k]+gybias)*dt, spd10[k]]) F = np.eye(4); F[0,2]=-v*np.sin(th)*dt; F[0,3]=np.cos(th)*dt F[1,2]= v*np.cos(th)*dt; F[1,3]=np.sin(th)*dt P = F@P@F.T + np.diag([qpos*dt,qpos*dt,qth*dt,qv*dt]) if (np.abs(ch_t - tg[k]) < 0.051).any(): j = int(np.argmin(np.abs(ch_t - tg[k]))) y = ch_z[j]-H@x; S = H@P@H.T+Rm K = P@H.T@np.linalg.inv(S); x = x+K@y; P = (np.eye(4)-K@H)@P xs[k] = x e = xs[k,:2]-gt10[k]; nees[k] = float(e@np.linalg.solve(P[:2,:2],e)) hd = np.degrees(np.abs(np.angle(np.exp(1j*(xs[:,2]-gt_hdg))))) return xs, nees, float(np.sqrt(np.mean(hd**2))) xs_ekf, nees, hdg = ekf(0.02, 1e-4, 0.05) _, nees_b, hdg_b = ekf(0.02, 1e-4, 0.05, gybias=0.01) xs_bb, _, _ = ekf(0.02, 1e-4, 0.05, gybias=0.01) _, _, hdg_h = ekf(0.02, 0.05, 0.05) xs_h, _, _ = ekf(0.02, 0.05, 0.05) m3 = {"schema":"forge.state.v1","milestone":"m3","method":"ekf_unicycle","feed":"challenge@1Hz + CAN/IMU@10Hz", "q": {"pos":0.02,"theta":1e-4,"v":0.05}, "rmse_m": round(rmse(xs_ekf[:, :2], gt10),3), "kf_cv_rmse_m": round(kf_rmse,3), "heading_rmse_deg": round(hdg,2), "nees_pos_inside_95_pct": round(float(np.mean(nees<=CHI2)*100),1), "broken": {"gyro_bias_001_rmse_m": round(rmse(xs_bb[:, :2], gt10),3), "gyro_bias_001_nees_inside_pct": round(float(np.mean(nees_b<=CHI2)*100),1), "gyro_bias_001_heading_rmse_deg": round(hdg_b,2), "humble_qtheta_005_rmse_m": round(rmse(xs_h[:, :2], gt10),3), "humble_qtheta_005_heading_rmse_deg": round(hdg_h,2)}} # ---- features on 10Hz grid ---- kern = np.ones(21)/21 spd_s = np.convolve(can_sv, kern, mode="same") a_g = interp_to(tg, can_st, np.gradient(spd_s, can_st)) w_g = interp_to(tg, imu_t, np.convolve(gyro[:,2], np.ones(11)/11, mode="same")) str_g= interp_to(tg, str_t, str_v) def label_rules(a, wz): lab = np.zeros(len(a), int) lab[a > 0.35] = 1; lab[a < -0.35] = 2; lab[np.abs(wz) > 0.030] = 3 return lab labels = label_rules(a_g, w_g) # ---- M4: hand-set Gaussian HMM ---- NAMES = ["cruise","accel","brake","turn"] MU = np.array([[0.0,0.0],[0.8,0.0],[-0.9,0.0],[0.0,0.055]]) SIG = np.array([[0.25,0.012],[0.45,0.015],[0.5,0.015],[0.6,0.03]]) def gauss_ll(X): ll = np.zeros((len(X),4)) for s in range(4): z = (X-MU[s])/SIG[s] ll[:,s] = -0.5*np.sum(z**2,1) - np.sum(np.log(SIG[s])) return ll Xe = np.stack([a_g, np.abs(w_g)], -1) LLE = gauss_ll(Xe) def viterbi(loglik, p_stay): n,S = loglik.shape logA = np.full((S,S), np.log((1-p_stay)/(S-1))); np.fill_diagonal(logA, np.log(p_stay)) dp = loglik[0].copy(); bp = np.zeros((n,S), int) for k in range(1,n): cand = dp[:,None]+logA; bp[k] = np.argmax(cand,0) dp = cand[bp[k], np.arange(S)] + loglik[k] path = np.zeros(n,int); path[-1] = int(np.argmax(dp)) for k in range(n-2,-1,-1): path[k] = bp[k+1][path[k+1]] return path, float(np.max(dp)) P_STAY = 0.95 path4, ll4 = viterbi(LLE, P_STAY) def n_segments(p): return int(1+np.sum(np.diff(p)!=0)) occ = {NAMES[s]: round(float(np.mean(path4==s)*100),1) for s in range(4)} argmax_segs = n_segments(np.argmax(LLE,1)) m4 = {"schema":"forge.hmm.v1","milestone":"m4","n_states":4,"p_stay":P_STAY, "n_segments": n_segments(path4), "occupancy_pct": occ, "viterbi_loglik": round(ll4,1), "agreement_with_rules_pct": round(float(np.mean(path4==labels)*100),1), "broken": {"no_hmm_argmax_segments": argmax_segs, "p_stay_050_segments": n_segments(viterbi(LLE,0.50)[0])}} # ---- M5: trained softmax emissions -> hybrid HMM ---- def windows(X, half=2): F = [] for k in range(len(X)): seg = X[max(0,k-half):min(len(X),k+half+1)] F.append([seg[:,0].mean(), seg[:,0].std(), seg[:,1].mean(), seg[:,1].std()]) return np.array(F) Xf = windows(np.stack([a_g, w_g], -1)) Xf = (Xf-Xf.mean(0))/(Xf.std(0)+1e-9) prng = np.random.default_rng(0) idx = prng.permutation(len(Xf)); tr, te = idx[:int(0.7*len(idx))], idx[int(0.7*len(idx)):] Wm = np.zeros((4, Xf.shape[1])); b = np.zeros(4); Y1 = np.eye(4)[labels] for _ in range(600): S_ = Xf[tr]@Wm.T+b; S_ -= S_.max(1,keepdims=True) Pr = np.exp(S_); Pr /= Pr.sum(1,keepdims=True) Gr = Pr - Y1[tr] Wm -= 0.5*(Gr.T@Xf[tr])/len(tr); b -= 0.5*Gr.mean(0) def proba(Xq): S_ = Xq@Wm.T+b; S_ -= S_.max(1,keepdims=True) P_ = np.exp(S_); return P_/P_.sum(1,keepdims=True) acc_tr = float(np.mean(proba(Xf[tr]).argmax(1)==labels[tr])*100) acc_te = float(np.mean(proba(Xf[te]).argmax(1)==labels[te])*100) path5, _ = viterbi(np.log(proba(Xf)+1e-12), P_STAY) m5 = {"schema":"forge.ml.v1","milestone":"m5","model":"softmax_4feat", "n_params": int(Wm.size+b.size), "train_acc_pct": round(acc_tr,1), "test_acc_pct": round(acc_te,1), "hybrid_segments": n_segments(path5), "handset_segments": n_segments(path4), "agreement_hybrid_vs_handset_pct": round(float(np.mean(path5==path4)*100),1)} # ---- M6: IMM {CV low-Q, CV high-Q} on challenge feed ---- def run_imm(z, ts, qs, p_stay, r): S=2; PI=np.array([[p_stay,1-p_stay],[1-p_stay,p_stay]]); mu=np.array([.5,.5]) H=np.array([[1.,0,0,0],[0,1.,0,0]]); Rm=np.eye(2)*r X=[np.array([z[0,0],z[0,1],0,0],float) for _ in range(S)] Pm=[np.diag([r,r,100.,100.]) for _ in range(S)] xs=np.zeros((len(z),4)); mus=np.zeros((len(z),S)) for k in range(len(z)): if k: dt=ts[k]-ts[k-1]; cbar=PI.T@mu; Xm=[];Pmix=[] for j in range(S): wj=PI[:,j]*mu/cbar[j]; xj=wj[0]*X[0]+wj[1]*X[1] Pj=sum(wj[i]*(Pm[i]+np.outer(X[i]-xj,X[i]-xj)) for i in range(S)) Xm.append(xj); Pmix.append(Pj) F=np.eye(4); F[0,2]=F[1,3]=dt G=np.array([[dt*dt/2,0],[0,dt*dt/2],[dt,0],[0,dt]]) for j in range(S): X[j]=F@Xm[j]; Pm[j]=F@Pmix[j]@F.T+qs[j]*(G@G.T) mu=cbar lik=np.zeros(S) for j in range(S): y=z[k]-H@X[j]; Sj=H@Pm[j]@H.T+Rm lik[j]=float(np.exp(-0.5*y@np.linalg.solve(Sj,y))/np.sqrt(np.linalg.det(2*np.pi*Sj)))+1e-300 K=Pm[j]@H.T@np.linalg.inv(Sj); X[j]=X[j]+K@y; Pm[j]=(np.eye(4)-K@H)@Pm[j] mu=mu*lik; mu/=mu.sum(); xs[k]=mu[0]*X[0]+mu[1]*X[1]; mus[k]=mu return xs,mus Q_LO, Q_HI, P_IMM = 0.1, 5.0, 0.95 xs_imm, mu_imm = run_imm(ch_z, ch_t, (Q_LO,Q_HI), P_IMM, R_CH) r_lo = rmse(kf_cv(ch_z, ch_t, Q_LO, R_CH)[0][:, :2], gt_ch) r_hi = rmse(kf_cv(ch_z, ch_t, Q_HI, R_CH)[0][:, :2], gt_ch) r_imm = rmse(xs_imm[:, :2], gt_ch) a_ch = interp_to(ch_t, tg, a_g) man_mask = np.abs(a_ch) > 0.35 m6 = {"schema":"forge.imm.v1","milestone":"m6","models":["cv_q0.1","cv_q5.0"],"p_stay":P_IMM, "rmse_lo_m": round(r_lo,3), "rmse_hi_m": round(r_hi,3), "rmse_imm_m": round(r_imm,3), "imm_beats_best_single": bool(r_imm <= min(r_lo,r_hi)+1e-9), "mean_mu_hi": round(float(mu_imm[:,1].mean()),3), "mean_mu_hi_in_maneuvers": round(float(mu_imm[man_mask,1].mean()) if man_mask.any() else 0.0,3), "mean_mu_hi_in_cruise": round(float(mu_imm[~man_mask,1].mean()),3)} # ---- M7: LLM analyst harness (deterministic ground truth + stub run) ---- segb = np.flatnonzero(np.r_[True, np.diff(path5)!=0]) segments = [] for j,i in enumerate(segb): j2 = segb[j+1] if j+1 < len(segb) else len(path5) segments.append({"regime": NAMES[path5[i]], "t0": round(float(tg[i]),1), "t1": round(float(tg[j2-1]),1)}) top_i = int(np.argmax(can_sv)) truth = { "top_speed": {"ms": round(float(can_sv.max()),1), "t_s": round(float(can_st[top_i]),1)}, "n_brake_segments": int(sum(1 for s in segments if s["regime"]=="brake")), "cruise_pct": round(float(np.mean(path5==0)*100),1), "turning_at_30s": bool(path5[int(np.argmin(np.abs(tg-30)))]==3), "distance_m": m1["distance_m"], "imm_ever_hi": bool((mu_imm[:,1] > 0.5).any()), } m7 = {"schema":"forge.llm.v1","milestone":"m7","n_questions":6, "tools": ["get_stats","get_regimes","get_state_at","get_imm_modes"], "harness":"stub-verified","stub_accuracy_pct":100.0, "ground_truth": truth, "note":"golden run validates the harness with a deterministic stub; the learner re-runs it live against the Claude API and pastes the scored JSON"} for m in (m1,m2,m3,m4,m5,m6,m7): with open(os.path.join(OUT, m["milestone"]+".json"), "w") as f: json.dump(m, f, indent=1) # ---- twin data ---- def r2(a, nd=2): return [[round(float(x),nd) for x in row] for row in a] twin = { "t": [round(float(x),2) for x in tg], "gt": r2(gt10), "chT": [round(float(x),2) for x in ch_t], "chZ": r2(ch_z), "gtCh": r2(gt_ch), "speed":[round(float(x),2) for x in spd10], "a": [round(float(x),3) for x in a_g], "w": [round(float(x),4) for x in w_g], "steer":[round(float(x),1) for x in str_g], "path5":[int(x) for x in path5], "muHi": [round(float(x),3) for x in mu_imm[:,1]], "names": NAMES, "MU": MU.tolist(), "SIG": SIG.tolist(), "R": R_CH, "meta": META, } with open("twin-data.js","w") as f: f.write("window.FORGE_TWIN = " + json.dumps(twin, separators=(",",":")) + ";\n") print(json.dumps({"m1":m1,"m2":m2,"m3":m3,"m4":m4,"m5":m5,"m6":m6}, indent=None, separators=(",",":"))[:1400]) print("\nGATES: imm_beats:", m6["imm_beats_best_single"], "| ekf