sensor fusion information interactive

%23%20%2F%2F%2F%20script%0A%23%20requires-python%20%3D%20%22%3E%3D3.11%22%0A%23%20dependencies%20%3D%20%5B%22marimo%3E%3D0.23.9%22%2C%20%22matplotlib%3D%3D3.10.8%22%2C%20%22numpy%3D%3D2.4.3%22%5D%0A%23%20%0A%23%20%5Btool.marimo.export%5D%0A%23%20lock_kind%20%3D%20%22resolved%22%0A%23%20%2F%2F%2F%0A%0Aimport%20marimo%0A%0A__generated_with%20%3D%20%220.25.1%22%0Aapp%20%3D%20marimo.App()%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt%0A%20%20%20%20import%20numpy%20as%20np%0A%0A%20%20%20%20plt.style.use(%22default%22)%0A%20%20%20%20BLUE%2C%20ORANGE%2C%20RED%2C%20GREY%2C%20GREEN%2C%20PURPLE%20%3D%20%22%231f77b4%22%2C%20%22%23ff7f0e%22%2C%20%22%23d62728%22%2C%20%220.55%22%2C%20%22%232ca02c%22%2C%20%22%239467bd%22%0A%20%20%20%20return%20BLUE%2C%20GREEN%2C%20GREY%2C%20ORANGE%2C%20RED%2C%20mo%2C%20np%2C%20plt%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%20Sensor%20Fusion%3A%20Precisions%20Add%0A%0A%20%20%20%20An%20unknown%20position%20%24%5Cmathbf%20Y%5Cin%5Cmathbb%20R%5E2%24%20is%20seen%20by%20two%20sensors.%20Each%20measures%20**one%20direction**%0A%20%20%20%20of%20it%2C%20%24X_i%3D%5Cmathbf%20h_i%5E%5Cmathsf%7BT%7D%5Cmathbf%20Y%2BN_i%24%20with%20%24%5Cmathbf%20h_i%3D%5B%5Ccos%5Cvarphi_i%2C%5C%20%5Csin%5Cvarphi_i%5D%5E%5Cmathsf%7BT%7D%24%20and%20noise%0A%20%20%20%20variance%20%24%5Csigma_i%5E2%24%2C%20and%20there%20is%20a%20weak%20prior%20%24%5Cmathbf%20Y%5Csim(%5Cmathbf%200%2C%5C%20p_0%5Cmathbf%20I)%24.%20The%20fused%20estimate%0A%20%20%20%20is%20best%20written%20in%20**information%20form**%3A%0A%0A%20%20%20%20%24%24%5Cmathbf%20P%5E%7B-1%7D%3D%5Cmathbf%20P_0%5E%7B-1%7D%2B%5Csum_i%5Cfrac%7B%5Cmathbf%20h_i%5Cmathbf%20h_i%5E%5Cmathsf%7BT%7D%7D%7B%5Csigma_i%5E2%7D%2C%5Cqquad%0A%20%20%20%20%5Cwidehat%7B%5Cmathbf%20Y%7D%3D%5Cmathbf%20P%5C%2C%5Csum_i%5Cfrac%7B%5Cmathbf%20h_i%5C%2Cx_i%7D%7B%5Csigma_i%5E2%7D.%24%24%0A%0A%20%20%20%20Each%20sensor%20adds%20a%20**rank-one**%20piece%20of%20information%3A%20certainty%20only%20along%20its%20own%20direction.%20Move%20the%0A%20%20%20%20sensors'%20directions%20and%20noise%20levels%20and%20watch%20the%20uncertainty%20ellipses%20(%242%5Csigma%24)%20combine.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20phi1%20%3D%20mo.ui.slider(0%2C%20180%2C%20step%3D1%2C%20value%3D10%2C%20label%3D%22sensor%201%20direction%20%CF%86%E2%82%81%20(%C2%B0)%22)%0A%20%20%20%20phi2%20%3D%20mo.ui.slider(0%2C%20180%2C%20step%3D1%2C%20value%3D80%2C%20label%3D%22sensor%202%20direction%20%CF%86%E2%82%82%20(%C2%B0)%22)%0A%20%20%20%20s1%20%3D%20mo.ui.slider(0.1%2C%202.0%2C%20step%3D0.05%2C%20value%3D0.4%2C%20label%3D%22sensor%201%20noise%20%CF%83%E2%82%81%22)%0A%20%20%20%20s2%20%3D%20mo.ui.slider(0.1%2C%202.0%2C%20step%3D0.05%2C%20value%3D0.8%2C%20label%3D%22sensor%202%20noise%20%CF%83%E2%82%82%22)%0A%20%20%20%20p0%20%3D%20mo.ui.slider(0.5%2C%2020.0%2C%20step%3D0.5%2C%20value%3D9.0%2C%20label%3D%22prior%20variance%20p%E2%82%80%22)%0A%20%20%20%20seq%20%3D%20mo.ui.checkbox(label%3D%22fuse%20sequentially%20(sensor%201%2C%20then%20sensor%202)%22%2C%20value%3DTrue)%0A%20%20%20%20mo.vstack(%5Bmo.hstack(%5Bphi1%2C%20s1%5D)%2C%20mo.hstack(%5Bphi2%2C%20s2%5D)%2C%20mo.hstack(%5Bp0%2C%20seq%5D)%5D)%0A%20%20%20%20return%20p0%2C%20phi1%2C%20phi2%2C%20s1%2C%20s2%2C%20seq%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20p0%2C%20phi1%2C%20phi2%2C%20s1%2C%20s2)%3A%0A%20%20%20%20def%20hvec(deg)%3A%0A%20%20%20%20%20%20%20%20return%20np.array(%5Bnp.cos(np.deg2rad(deg))%2C%20np.sin(np.deg2rad(deg))%5D)%0A%0A%20%20%20%20h1%2C%20h2%20%3D%20hvec(phi1.value)%2C%20hvec(phi2.value)%0A%20%20%20%20J0%20%3D%20np.eye(2)%20%2F%20p0.value%0A%20%20%20%20J1%20%3D%20np.outer(h1%2C%20h1)%20%2F%20s1.value**2%0A%20%20%20%20J2%20%3D%20np.outer(h2%2C%20h2)%20%2F%20s2.value**2%0A%20%20%20%20P0%2C%20P1%2C%20P2%2C%20Pf%20%3D%20(np.linalg.inv(J)%20for%20J%20in%20(J0%2C%20J0%20%2B%20J1%2C%20J0%20%2B%20J2%2C%20J0%20%2B%20J1%20%2B%20J2))%0A%20%20%20%20y_true%20%3D%20np.array(%5B1.2%2C%20-0.6%5D)%0A%20%20%20%20_rng%20%3D%20np.random.default_rng(11)%0A%20%20%20%20x1%20%3D%20h1%20%40%20y_true%20%2B%20s1.value%20*%20_rng.standard_normal()%0A%20%20%20%20x2%20%3D%20h2%20%40%20y_true%20%2B%20s2.value%20*%20_rng.standard_normal()%0A%20%20%20%20y1%20%3D%20P1%20%40%20(h1%20*%20x1%20%2F%20s1.value**2)%0A%20%20%20%20y2%20%3D%20P2%20%40%20(h2%20*%20x2%20%2F%20s2.value**2)%0A%20%20%20%20yf%20%3D%20Pf%20%40%20(h1%20*%20x1%20%2F%20s1.value**2%20%2B%20h2%20*%20x2%20%2F%20s2.value**2)%0A%20%20%20%20return%20P0%2C%20P1%2C%20P2%2C%20Pf%2C%20h1%2C%20h2%2C%20x2%2C%20y1%2C%20y2%2C%20y_true%2C%20yf%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20BLUE%2C%0A%20%20%20%20GREEN%2C%0A%20%20%20%20GREY%2C%0A%20%20%20%20ORANGE%2C%0A%20%20%20%20P0%2C%0A%20%20%20%20P1%2C%0A%20%20%20%20P2%2C%0A%20%20%20%20Pf%2C%0A%20%20%20%20RED%2C%0A%20%20%20%20h1%2C%0A%20%20%20%20h2%2C%0A%20%20%20%20mo%2C%0A%20%20%20%20np%2C%0A%20%20%20%20plt%2C%0A%20%20%20%20s2%2C%0A%20%20%20%20seq%2C%0A%20%20%20%20x2%2C%0A%20%20%20%20y1%2C%0A%20%20%20%20y2%2C%0A%20%20%20%20y_true%2C%0A%20%20%20%20yf%2C%0A)%3A%0A%20%20%20%20_th%20%3D%20np.linspace(0%2C%202%20*%20np.pi%2C%20300)%0A%0A%20%20%20%20def%20ell(ax%2C%20m%2C%20P%2C%20**kw)%3A%0A%20%20%20%20%20%20%20%20w%2C%20V%20%3D%20np.linalg.eigh(P)%0A%20%20%20%20%20%20%20%20E%20%3D%202%20*%20(V%20%40%20np.diag(np.sqrt(np.maximum(w%2C%200)))%20%40%20np.vstack(%5Bnp.cos(_th)%2C%20np.sin(_th)%5D))%0A%20%20%20%20%20%20%20%20ax.plot(m%5B0%5D%20%2B%20E%5B0%5D%2C%20m%5B1%5D%20%2B%20E%5B1%5D%2C%20**kw)%0A%0A%20%20%20%20fig%2C%20(a1%2C%20a2)%20%3D%20plt.subplots(1%2C%202%2C%20figsize%3D(12.5%2C%205.6))%0A%20%20%20%20ell(a1%2C%20np.zeros(2)%2C%20P0%2C%20color%3DGREY%2C%20lw%3D1.2%2C%20ls%3D%22%3A%22%2C%20label%3D%22prior%22)%0A%20%20%20%20ell(a1%2C%20y1%2C%20P1%2C%20color%3DORANGE%2C%20lw%3D2%2C%20label%3D%22sensor%201%20(%2Bprior)%22)%0A%20%20%20%20ell(a1%2C%20y2%2C%20P2%2C%20color%3DGREEN%2C%20lw%3D2%2C%20label%3D%22sensor%202%20(%2Bprior)%22)%0A%20%20%20%20ell(a1%2C%20yf%2C%20Pf%2C%20color%3DBLUE%2C%20lw%3D3%2C%20label%3D%22fused%22)%0A%20%20%20%20for%20_h%2C%20_c%20in%20%5B(h1%2C%20ORANGE)%2C%20(h2%2C%20GREEN)%5D%3A%0A%20%20%20%20%20%20%20%20a1.annotate(%22%22%2C%20xy%3D1.8%20*%20_h%2C%20xytext%3D(0%2C%200)%2C%20arrowprops%3Ddict(arrowstyle%3D%22-%3E%22%2C%20color%3D_c%2C%20lw%3D2))%0A%20%20%20%20a1.plot(*y_true%2C%20%22*%22%2C%20color%3DRED%2C%20ms%3D16%2C%20label%3D%22truth%22)%0A%20%20%20%20a1.plot(*yf%2C%20%22o%22%2C%20color%3DBLUE%2C%20ms%3D8)%0A%20%20%20%20a1.set_xlim(-5%2C%205)%3B%20a1.set_ylim(-5%2C%205)%3B%20a1.set_aspect(%22equal%22)%0A%20%20%20%20a1.legend(fontsize%3D9%2C%20frameon%3DFalse%2C%20loc%3D%22lower%20left%22)%0A%20%20%20%20a1.set_title(%222%CF%83%20uncertainty%20ellipses%3B%20arrows%3A%20the%20direction%20each%20sensor%20sees%22%2C%20fontsize%3D11)%0A%0A%20%20%20%20_names%20%3D%20%5B%22prior%22%2C%20%22sensor%201%22%2C%20%22sensor%202%22%2C%20%22fused%22%5D%0A%20%20%20%20_tr%20%3D%20%5Bnp.trace(P)%20for%20P%20in%20(P0%2C%20P1%2C%20P2%2C%20Pf)%5D%0A%20%20%20%20a2.bar(_names%2C%20_tr%2C%20color%3D%5BGREY%2C%20ORANGE%2C%20GREEN%2C%20BLUE%5D)%0A%20%20%20%20for%20_i%2C%20_v%20in%20enumerate(_tr)%3A%0A%20%20%20%20%20%20%20%20a2.text(_i%2C%20_v%20*%201.05%2C%20f%22%7B_v%3A.2f%7D%22%2C%20ha%3D%22center%22%2C%20fontsize%3D11)%0A%20%20%20%20a2.set_yscale(%22log%22)%3B%20a2.set_ylabel(r%22total%20error%20variance%20tr%20%24%5Cmathbf%7BP%7D%24%22)%0A%20%20%20%20a2.set_title(%22how%20uncertain%2C%20in%20total%22%2C%20fontsize%3D11)%0A%20%20%20%20fig.tight_layout()%0A%0A%20%20%20%20_cosang%20%3D%20abs(h1%20%40%20h2)%0A%20%20%20%20_msg%20%3D%20(f%22The%20two%20sensors%20look%20along%20directions%20%7Bnp.degrees(np.arccos(_cosang))%3A.0f%7D%C2%B0%20apart.%20%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%2B%20(%22They%20see%20almost%20the%20same%20direction%2C%20so%20their%20information%20adds%20up%20along%20it%20and%20the%20perpendicular%20direction%20is%20left%20to%20the%20prior.%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20if%20_cosang%20%3E%200.95%20else%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%22Each%20alone%20leaves%20one%20direction%20to%20the%20prior%3B%20together%20they%20pin%20the%20unknown%20down%20in%20both.%22))%0A%20%20%20%20_out%20%3D%20%5Bmo.md(_msg)%2C%20fig%5D%0A%20%20%20%20if%20seq.value%3A%0A%20%20%20%20%20%20%20%20_K%20%3D%20P1%20%40%20h2%20%2F%20(h2%20%40%20P1%20%40%20h2%20%2B%20s2.value**2)%0A%20%20%20%20%20%20%20%20_yseq%20%3D%20y1%20%2B%20_K%20*%20(x2%20-%20h2%20%40%20y1)%0A%20%20%20%20%20%20%20%20_out.append(mo.md(%0A%20%20%20%20%20%20%20%20%20%20%20%20%22**Sequentially%3A**%20start%20from%20sensor%201's%20estimate%20and%20covariance%2C%20then%20fold%20in%20sensor%202%20with%20the%20gain%20%22%0A%20%20%20%20%20%20%20%20%20%20%20%20rf%22%24%5Cmathbf%20K%3D%5Cmathbf%20P_1%5Cmathbf%20h_2%2F(%5Cmathbf%20h_2%5E%5Cmathsf%7B%7BT%7D%7D%5Cmathbf%20P_1%5Cmathbf%20h_2%2B%5Csigma_2%5E2)%24%20%3D%20%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%5B%7B_K%5B0%5D%3A.3f%7D%2C%20%7B_K%5B1%5D%3A.3f%7D%5D.%20Sequential%20estimate%20%5B%7B_yseq%5B0%5D%3A.3f%7D%2C%20%7B_yseq%5B1%5D%3A.3f%7D%5D%2C%20batch%20estimate%20%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%5B%7Byf%5B0%5D%3A.3f%7D%2C%20%7Byf%5B1%5D%3A.3f%7D%5D%3A%20identical.%20The%20recursion%20only%20avoids%20recomputing.%22%0A%20%20%20%20%20%20%20%20))%0A%20%20%20%20mo.vstack(_out)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20What%20to%20try%0A%0A%20%20%20%20-%20**Make%20the%20two%20directions%20perpendicular**%20(e.g.%200%C2%B0%20and%2090%C2%B0)%3A%20each%20sensor%20covers%20the%20other's%20blind%20direction%2C%20and%20the%20fused%20ellipse%20is%20small%20in%20both.%0A%20%20%20%20-%20**Make%20them%20parallel**%3A%20the%20fused%20ellipse%20stays%20long%20in%20the%20unobserved%20direction%20however%20good%20the%20sensors%20are.%20Only%20the%20prior%20limits%20it%20there%3B%20shrink%20%24p_0%24%20to%20see%20it.%0A%20%20%20%20-%20**Make%20one%20sensor%20bad**%20(%24%5Csigma%5Cto2%24)%3A%20its%20ellipse%20balloons%20along%20its%20direction%20and%20it%20hardly%20moves%20the%20fused%20estimate.%20A%20useless%20sensor%20drops%20out%20by%20itself%3B%20it%20is%20never%20harmful.%0A%20%20%20%20-%20**Tick%20sequential%20fusion**%3A%20the%20gain%20that%20folds%20sensor%202%20into%20sensor%201's%20estimate%20is%20the%20Kalman%20gain%2C%20one%20measurement%20at%20a%20time.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0Aif%20__name__%20%3D%3D%20%22__main__%22%3A%0A%20%20%20%20app.run()%0A