# Generated by tools/build_playground_sources.py; do not edit. # Original source SHA-256: f9432925e730a68ca265e2fbcceb34c1d9531fede9126b073452d5ee178076fd import numpy as np OBSERVATIONS = [0.2133019558, -0.6279888744, 0.7253158371, 0.9583953015, -0.9657246321, -0.4115256548, 0.6894882822, 0.4786301854, 0.7882391897, 0.3028692507, 1.615578582, 1.644454355, 1.246221488, 2.089068845, 1.72725654, 0.898495276, 1.858125549, 1.028782179, 2.414915211, 1.865051862, 1.870596346, 1.623349319, 3.055778937, 2.191829363, 2.100170524, 2.253506515, 2.97261643, 2.955810845, 3.088912828, 3.201574702, 4.499153321, 2.815509489, 2.84143009, 2.73035909, 3.831185596, 4.290280605, 3.52023678, 3.111890466, 3.222863149, 4.355414951, 4.52027792, 4.480207988, 3.734143205, 4.462512926, 4.481680066, 4.653082018, 5.210000145, 4.856516884, 5.275239494, 4.947305349, 5.202383579, 5.541901758, 4.179990926, 5.076230149, 5.070739142, 5.052785506, 5.407400424, 6.746458918, 5.193918219, 6.577794848, 4.82199116, 5.865580479, 6.313927146, 6.710355632, 6.897858606, 7.055343065, 6.355892449, 6.376353745, 7.400583117, 6.766086973, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, None, 9.735286212, 11.00945981, 9.95061522, 10.8162609, 9.746467624, 10.35619371, 9.934984562, 10.46267685, 11.3882157, 9.690875704, 11.30409655, 11.26641492, 10.78409503, 10.2877595, 11.45049066, 11.1293551, 11.76287335, 11.7152965, 12.92124522, 11.73245106, 11.28355176, 12.26549294, 12.43399768, 13.3714313, 13.14457787, 12.94980974, 13.86431202, 12.14786586, 12.67217393, 12.61139684, 13.12713314, 12.5763197, 14.12460566, 13.66444411, 12.93043559, 13.38909464, 14.45945969, 14.9666886, 15.91771162, 16.69970373, 15.0900866, 14.24732332, 13.5875676, 15.40739802, 14.79094123, 15.20924992, 15.21153224, 15.68144638, 16.66618616, 16.169934, 16.08895561, 15.61504237, 15.30772194, 16.27958446, 16.72235221, 18.13755094, 17.13119217, 17.86791766, 16.97049308, 16.63053936, 28.92441827, 17.23234175, 19.36992881, 17.44502932, 18.74694244, 17.66795098, 19.09210111, 18.84946568, 18.61035347, 18.83146623, 18.54164861, 19.45225054, 18.96151156, 18.56207597, 18.6654437, 19.82081154, 20.94536388, 20.09199413, 20.03695317, 20.4600783, 21.31420122, 20.69356775, 20.39235094, 21.5944021, 21.26012951, 22.17502919, 21.36826411, 20.52287168, 20.56228856, 22.81564955, 23.006566, 21.81433655, 21.81176888, 23.243011, 21.58506802, 21.87369109, 23.09032876, 22.50377641, 22.91641469, 22.94558997, 23.43630218, 24.3252373, 23.54340943, 24.07075716, 22.32487953, 23.86589712, 23.44973881, 23.32683086, 23.70529334, 24.22611359, 25.24113178, 23.8115251, 24.90144204, 24.6810814, 24.93062883, 26.00193048, 25.81668081, 26.51617868, 25.61184602, 25.37284017, 25.84329883, 26.30974775, 26.40360135, 25.66092835, 26.62334285, 26.85975983, 28.60223183, 28.29379123, 26.52272965, 27.05883165, 26.3755906, 27.12650509, 27.9009235, 28.66409753, 27.44964131, 27.64209749, 26.73689768, 28.26613386, 27.77630991, 28.2893924, 28.18619746, 28.87401621, 27.84959013, 28.19306833, 30.85047298, 28.59880419, 28.8722501, 31.06583947, 31.95354702, 29.23990336, 29.94222573, 30.57908889, 31.69008835, 29.92920005, 30.58830551, 31.4441363, 31.34433625, 30.91669075, 31.22632392, 30.49757293, 31.43327838, 31.55352876, 32.04251892, 31.63127095, 32.49007697, 33.00890107, 32.54880053, 32.82622949, 32.75720874, 32.86005908, 32.49490938, 33.36154598, 33.21189938, 34.88521782, 34.66134843, 33.97009259, 33.30585995, 33.20131197, 34.95380007, 34.44392446, 34.73610038, 33.31878981, 35.32920694, 35.13809424, 34.18269852, 34.76993263, 35.42460204, 35.41672676, 35.31548017, 35.58755821, 35.62361584, 36.04679376, 37.11004438, 34.42333909, 36.19420481, 36.62355869, 36.84719579, 36.51965979, 35.69029475, 37.28959684] def kalman_values(q=.4, r=.49, gate=False): rng = np.random.default_rng(42) dt = 0.1 t = np.arange(300) * dt truth = t + np.maximum(t - 12, 0) * 0.4 z = truth + rng.normal(0, 0.7, len(t)) z[160] += 12 z[70:100] = np.nan z = np.array(OBSERVATIONS, dtype=float) F = np.array([[1, dt], [0, 1]]) h = np.array([1.0, 0.0]) I = np.eye(2) x = np.array([0.0, 1.0]) P = np.diag([1.0, 1.0]) vals = [] sig = [] rejected = 0 g = np.array([dt ** 2 / 2, dt]) Q = q * np.outer(g, g) for measurement in z: x = F @ x P = F @ P @ F.T + Q if np.isfinite(measurement): e = measurement - h @ x S = h @ P @ h + r if gate and e * e / S > 9: rejected += 1 else: K = P @ h / S x = x + K * e A = I - np.outer(K, h) P = A @ P @ A.T + np.outer(K, K) * r assert np.linalg.eigvalsh(P).min() > -1e-12 vals.append(x[0]) sig.append(np.sqrt(P[0, 0])) vals = np.array(vals) return t, truth, z, vals, np.array(sig), rejected def rotation(a): return np.array([[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]) def icp_fit(source, target, angle, translation, threshold): R = rotation(angle) u = np.array(translation, dtype=float) for _ in range(100): p = source @ R.T + u d = np.linalg.norm(p[:, None, :] - target[None, :, :], axis=2) idx = d.argmin(axis=1) dist = d[np.arange(len(p)), idx] mask = dist < threshold if mask.sum() < 3: break a = p[mask] b = target[idx[mask]] ac = a.mean(axis=0) bc = b.mean(axis=0) U, _, Vt = np.linalg.svd((a - ac).T @ (b - bc)) D = np.diag([1.0, np.linalg.det(Vt.T @ U.T)]) delta = Vt.T @ D @ U.T shift = bc - delta @ ac R = delta @ R u = delta @ u + shift if np.linalg.norm(shift) + np.linalg.norm(delta - np.eye(2)) < 1e-10: break p = source @ R.T + u dist = np.linalg.norm(p[:, None, :] - target[None, :, :], axis=2).min(axis=1) mask = dist < threshold return (p, R, u, float(np.sqrt(np.mean(dist[mask] ** 2))) if mask.any() else None, float(mask.mean()))