kalman tracking 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%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%0A%20%20%20%20return%20BLUE%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%20The%20Kalman%20Filter%3A%20Predict%2C%20Then%20Fuse%0A%0A%20%20%20%20A%20train%20moves%20along%20a%20track%20with%20random%20accelerations.%20Once%20per%20second%20a%20receiver%20reports%20its%0A%20%20%20%20**position**%20with%20noise%20of%20standard%20deviation%20%24%5Csigma_V%24%3B%20nothing%20measures%20the%20velocity.%20The%20true%20motion%20is%0A%20%20%20%20the%20constant-velocity%20model%0A%0A%20%20%20%20%24%24%5Cmathbf%20S%5Bk%5D%3D%5Cbegin%7Bpmatrix%7D1%26T%5C%5C0%261%5Cend%7Bpmatrix%7D%5Cmathbf%20S%5Bk-1%5D%2B%5Cbegin%7Bpmatrix%7DT%5E2%2F2%5C%5CT%5Cend%7Bpmatrix%7DU%5Bk%5D%2C%5Cqquad%0A%20%20%20%20X%5Bk%5D%3D%5Cbegin%7Bpmatrix%7D1%260%5Cend%7Bpmatrix%7D%5Cmathbf%20S%5Bk%5D%2BV%5Bk%5D%2C%24%24%0A%0A%20%20%20%20with%20acceleration%20noise%20of%20standard%20deviation%20%24%5Csigma_U%24.%20The%20filter%20**predicts**%20through%20its%20model%2C%20then%0A%20%20%20%20**fuses**%20the%20new%20fix.%20You%20choose%20the%20model%20the%20filter%20believes%3A%20the%20true%20one%2C%20or%20a%20random%20walk%20that%0A%20%20%20%20assumes%20the%20train%20stays%20where%20it%20was%2C%20and%20the%20process%20noise%20the%20filter%20assumes.%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%20su%20%3D%20mo.ui.slider(0.01%2C%201.0%2C%20step%3D0.01%2C%20value%3D0.15%2C%20label%3D%22true%20acceleration%20noise%20%CF%83_U%22)%0A%20%20%20%20sv%20%3D%20mo.ui.slider(0.1%2C%2010.0%2C%20step%3D0.1%2C%20value%3D3.0%2C%20label%3D%22measurement%20noise%20%CF%83_V%22)%0A%20%20%20%20model%20%3D%20mo.ui.dropdown(%5B%22constant%20velocity%22%2C%20%22random%20walk%22%5D%2C%20value%3D%22constant%20velocity%22%2C%20label%3D%22filter's%20model%22)%0A%20%20%20%20qscale%20%3D%20mo.ui.slider(-3.0%2C%203.0%2C%20step%3D0.25%2C%20value%3D0.0%2C%20label%3D%22assumed%20process%20noise%20(log%E2%82%81%E2%82%80%20%C3%97%20true)%22)%0A%20%20%20%20seed%20%3D%20mo.ui.slider(0%2C%2020%2C%20step%3D1%2C%20value%3D3%2C%20label%3D%22random%20seed%22)%0A%20%20%20%20mo.vstack(%5Bmo.hstack(%5Bsu%2C%20sv%5D)%2C%20mo.hstack(%5Bmodel%2C%20qscale%2C%20seed%5D)%5D)%0A%20%20%20%20return%20model%2C%20qscale%2C%20seed%2C%20su%2C%20sv%0A%0A%0A%40app.cell%0Adef%20_(model%2C%20np%2C%20qscale%2C%20seed%2C%20su%2C%20sv)%3A%0A%20%20%20%20T%2C%20n%20%3D%201.0%2C%20120%0A%20%20%20%20A_true%20%3D%20np.array(%5B%5B1.0%2C%20T%5D%2C%20%5B0.0%2C%201.0%5D%5D)%0A%20%20%20%20B%20%3D%20np.array(%5BT**2%20%2F%202%2C%20T%5D)%0A%20%20%20%20_rng%20%3D%20np.random.default_rng(seed.value)%0A%20%20%20%20S%20%3D%20np.zeros((n%2C%202))%3B%20s%20%3D%20np.array(%5B0.0%2C%203.0%5D)%0A%20%20%20%20for%20_k%20in%20range(n)%3A%0A%20%20%20%20%20%20%20%20s%20%3D%20A_true%20%40%20s%20%2B%20B%20*%20su.value%20*%20_rng.standard_normal()%0A%20%20%20%20%20%20%20%20S%5B_k%5D%20%3D%20s%0A%20%20%20%20X%20%3D%20S%5B%3A%2C%200%5D%20%2B%20sv.value%20*%20_rng.standard_normal(n)%0A%0A%20%20%20%20q_assumed%20%3D%20(su.value%20*%2010%20**%20qscale.value)%20**%202%0A%20%20%20%20if%20model.value%20%3D%3D%20%22constant%20velocity%22%3A%0A%20%20%20%20%20%20%20%20A%2C%20Q%2C%20H%20%3D%20A_true%2C%20np.outer(B%2C%20B)%20*%20q_assumed%2C%20np.array(%5B1.0%2C%200.0%5D)%0A%20%20%20%20%20%20%20%20sh%2C%20P%20%3D%20np.array(%5BX%5B0%5D%2C%200.0%5D)%2C%20np.diag(%5Bsv.value**2%2C%2025.0%5D)%0A%20%20%20%20else%3A%0A%20%20%20%20%20%20%20%20%23%20random%20walk%20on%20position%20alone%3A%20it%20believes%20the%20train%20stays%20where%20it%20was%0A%20%20%20%20%20%20%20%20A%2C%20Q%2C%20H%20%3D%20np.eye(1)%2C%20np.eye(1)%20*%20q_assumed%2C%20np.array(%5B1.0%5D)%0A%20%20%20%20%20%20%20%20sh%2C%20P%20%3D%20np.array(%5BX%5B0%5D%5D)%2C%20np.eye(1)%20*%20sv.value**2%0A%20%20%20%20est%2C%20var%2C%20gain%2C%20nu_n%20%3D%20np.zeros((n%2C%202))%2C%20np.zeros(n)%2C%20np.zeros(n)%2C%20np.zeros(n)%0A%20%20%20%20for%20_k%20in%20range(n)%3A%0A%20%20%20%20%20%20%20%20sh%20%3D%20A%20%40%20sh%3B%20P%20%3D%20A%20%40%20P%20%40%20A.T%20%2B%20Q%0A%20%20%20%20%20%20%20%20Snu%20%3D%20H%20%40%20P%20%40%20H%20%2B%20sv.value**2%0A%20%20%20%20%20%20%20%20K%20%3D%20P%20%40%20H%20%2F%20Snu%0A%20%20%20%20%20%20%20%20nu%20%3D%20X%5B_k%5D%20-%20H%20%40%20sh%0A%20%20%20%20%20%20%20%20nu_n%5B_k%5D%20%3D%20nu%20%2F%20np.sqrt(Snu)%0A%20%20%20%20%20%20%20%20sh%20%3D%20sh%20%2B%20K%20*%20nu%3B%20P%20%3D%20P%20-%20np.outer(K%2C%20H%20%40%20P)%0A%20%20%20%20%20%20%20%20est%5B_k%2C%20%3Alen(sh)%5D%20%3D%20sh%3B%20var%5B_k%5D%20%3D%20P%5B0%2C%200%5D%3B%20gain%5B_k%5D%20%3D%20K%5B0%5D%0A%20%20%20%20rmse_est%20%3D%20np.sqrt(np.mean((est%5B20%3A%2C%200%5D%20-%20S%5B20%3A%2C%200%5D)%20**%202))%0A%20%20%20%20rmse_raw%20%3D%20np.sqrt(np.mean((X%5B20%3A%5D%20-%20S%5B20%3A%2C%200%5D)%20**%202))%0A%20%20%20%20return%20S%2C%20X%2C%20est%2C%20gain%2C%20n%2C%20nu_n%2C%20rmse_est%2C%20rmse_raw%2C%20var%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20BLUE%2C%0A%20%20%20%20GREY%2C%0A%20%20%20%20ORANGE%2C%0A%20%20%20%20RED%2C%0A%20%20%20%20S%2C%0A%20%20%20%20X%2C%0A%20%20%20%20est%2C%0A%20%20%20%20gain%2C%0A%20%20%20%20mo%2C%0A%20%20%20%20model%2C%0A%20%20%20%20n%2C%0A%20%20%20%20np%2C%0A%20%20%20%20nu_n%2C%0A%20%20%20%20plt%2C%0A%20%20%20%20rmse_est%2C%0A%20%20%20%20rmse_raw%2C%0A%20%20%20%20var%2C%0A)%3A%0A%20%20%20%20k%20%3D%20np.arange(n)%0A%20%20%20%20fig%2C%20axes%20%3D%20plt.subplots(2%2C%202%2C%20figsize%3D(13%2C%207.5))%0A%20%20%20%20a%20%3D%20axes%5B0%2C%200%5D%0A%20%20%20%20_sd%20%3D%20np.sqrt(var)%0A%20%20%20%20a.fill_between(k%2C%20-2%20*%20_sd%2C%202%20*%20_sd%2C%20color%3DBLUE%2C%20alpha%3D0.15%2C%20label%3D%22filter's%20own%20%C2%B12%CF%83%22)%0A%20%20%20%20a.plot(k%2C%20X%20-%20S%5B%3A%2C%200%5D%2C%20%22.%22%2C%20color%3DORANGE%2C%20ms%3D5%2C%20label%3D%22error%20of%20the%20raw%20fixes%22)%0A%20%20%20%20a.plot(k%2C%20est%5B%3A%2C%200%5D%20-%20S%5B%3A%2C%200%5D%2C%20color%3DBLUE%2C%20lw%3D2%2C%20label%3D%22error%20of%20the%20Kalman%20estimate%22)%0A%20%20%20%20a.axhline(0%2C%20color%3DRED%2C%20lw%3D1)%0A%20%20%20%20a.set_xlabel(%22time%20%24k%24%22)%3B%20a.set_ylabel(%22position%20error%22)%3B%20a.legend(fontsize%3D9%2C%20frameon%3DFalse%2C%20loc%3D%22upper%20right%22)%0A%20%20%20%20a.set_title(f%22RMS%20error%3A%20raw%20fixes%20%7Brmse_raw%3A.2f%7D%2C%20filter%20%7Brmse_est%3A.2f%7D%22%2C%20fontsize%3D11)%0A%0A%20%20%20%20a%20%3D%20axes%5B0%2C%201%5D%0A%20%20%20%20a.plot(k%2C%20S%5B%3A%2C%201%5D%2C%20color%3DRED%2C%20lw%3D2%2C%20label%3D%22true%20velocity%22)%0A%20%20%20%20if%20model.value%20%3D%3D%20%22constant%20velocity%22%3A%0A%20%20%20%20%20%20%20%20a.plot(k%2C%20est%5B%3A%2C%201%5D%2C%20color%3DBLUE%2C%20lw%3D2%2C%20label%3D%22estimated%20velocity%20(never%20measured)%22)%0A%20%20%20%20else%3A%0A%20%20%20%20%20%20%20%20a.text(0.5%2C%200.5%2C%20%22the%20random-walk%20model%5Cnhas%20no%20velocity%20to%20estimate%22%2C%20transform%3Da.transAxes%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20ha%3D%22center%22%2C%20va%3D%22center%22%2C%20fontsize%3D13%2C%20color%3DGREY)%0A%20%20%20%20a.set_xlabel(%22time%20%24k%24%22)%3B%20a.set_ylabel(%22velocity%22)%3B%20a.legend(fontsize%3D9%2C%20frameon%3DFalse)%0A%0A%20%20%20%20a%20%3D%20axes%5B1%2C%200%5D%0A%20%20%20%20a.plot(k%2C%20gain%2C%20color%3DORANGE%2C%20lw%3D2)%0A%20%20%20%20a.set_ylim(0%2C%201.05)%3B%20a.set_xlabel(%22time%20%24k%24%22)%3B%20a.set_ylabel(%22position%20gain%20%24K%24%22)%0A%20%20%20%20a.set_title(%22the%20gain%20settles%3A%20a%20fixed%20blend%20of%20model%20and%20measurement%22%2C%20fontsize%3D11)%0A%0A%20%20%20%20a%20%3D%20axes%5B1%2C%201%5D%0A%20%20%20%20_v%20%3D%20nu_n%5B10%3A%5D%20%20%23%20no%20mean%20removal%3A%20a%20lagging%20model%20shows%20up%20as%20a%20nonzero%20mean%0A%20%20%20%20_r%20%3D%20np.correlate(_v%2C%20_v%2C%20%22full%22)%5Blen(_v)%20-%201%3Alen(_v)%20%2B%2015%5D%20%2F%20len(_v)%0A%20%20%20%20a.stem(np.arange(16)%2C%20_r%2C%20basefmt%3D%22%20%22)%0A%20%20%20%20a.axhspan(-2%20%2F%20np.sqrt(len(_v))%2C%202%20%2F%20np.sqrt(len(_v))%2C%20color%3DGREY%2C%20alpha%3D0.2)%0A%20%20%20%20a.axhline(1.0%2C%20color%3DRED%2C%20lw%3D1%2C%20ls%3D%22%3A%22)%0A%20%20%20%20a.set_xlabel(r%22lag%20%24%5Ckappa%24%22)%3B%20a.set_ylabel(%22autocorrelation%22)%0A%20%20%20%20a.set_title(f%22normalized%20innovations%3A%20mean%20%7B_v.mean()%3A%2B.2f%7D%20(should%20be%200)%2C%20mean%20square%20%7Bnp.mean(_v**2)%3A.2f%7D%20(should%20be%201)%22%2C%20fontsize%3D10)%0A%20%20%20%20fig.tight_layout()%0A%20%20%20%20mo.vstack(%5B%0A%20%20%20%20%20%20%20%20mo.md(%22If%20the%20filter's%20model%20is%20right%2C%20its%20innovations%20are%20white%20with%20unit%20variance%20(bottom%20right).%20%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%22Correlated%20or%20oversized%20innovations%20are%20the%20filter's%20own%20report%20that%20its%20model%20is%20wrong.%22)%2C%0A%20%20%20%20%20%20%20%20fig%2C%0A%20%20%20%20%5D)%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**Switch%20to%20the%20random-walk%20model.**%20It%20predicts%20%22the%20train%20stays%20where%20it%20was%22%2C%20so%20it%20lags%20behind%20at%20every%20step%3A%20the%20innovations%20become%20correlated%20(they%20keep%20having%20the%20same%20sign)%20and%20larger%20than%20one.%0A%20%20%20%20-%20**Shrink%20the%20assumed%20process%20noise**%20(slider%20to%20the%20left).%20The%20filter%20trusts%20its%20model%20too%20much%2C%20the%20gain%20drops%2C%20and%20it%20responds%20sluggishly%3B%20the%20innovations%20tell%20on%20it.%0A%20%20%20%20-%20**Inflate%20it**%20(slider%20to%20the%20right).%20The%20gain%20goes%20toward%20one%20and%20the%20filter%20just%20follows%20the%20noisy%20fixes%3A%20the%20estimate%20is%20barely%20better%20than%20the%20raw%20measurements.%0A%20%20%20%20-%20**Raise%20the%20measurement%20noise.**%20The%20gain%20drops%20by%20itself%3A%20noisier%20fixes%20are%20trusted%20less%2C%20with%20no%20tuning.%0A%20%20%20%20-%20**Look%20at%20the%20velocity%20panel.**%20Nothing%20measures%20velocity%2C%20yet%20the%20constant-velocity%20filter%20estimates%20it%2C%20because%20the%20model%20links%20position%20changes%20to%20it.%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