kalman filter 01

import%20marimo%0A%0A__generated_with%20%3D%20%220.19.4%22%0Aapp%20%3D%20marimo.App(width%3D%22medium%22)%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%20%20%20%20import%20numpy%20as%20np%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt%0A%20%20%20%20return%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%20Kalman%20filter%0A%0A%20%20%20%20A%20discussion%20on%20how%20Kalman%20filters%20work%20and%20a%20short%20example%20of%20its%20implementation.%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Introduction%0A%0A%20%20%20%20Motivation%3A%20real-time%20estimation%20of%20a%20fly%20location%20to%20accurate%20control%20and%20image%20its%20brain.%0A%0A%20%20%20%20Paper%20reference%3A%20https%3A%2F%2Fwww.sciencedirect.com%2Fscience%2Farticle%2Fpii%2FS2211124718315766%0A%0A%20%20%20%20Here%2C%20the%20larvae's%20location%20was%20continuous%20estimated%20(and%20updated)%20using%20a%20Kalman%20filter.%20This%20location%20was%20used%20to%20image%20the%20larvae's%20brain%20(image%20below%20from%20paper).%20My%20aim%20is%20to%20apply%20a%20similar%20approach%20to%20estimate%20the%20fly%20location%20in%20real-time.%0A%0A%20%20%20%20%3Cimg%20src%3D%22https%3A%2F%2Fars.els-cdn.com%2Fcontent%2Fimage%2F1-s2.0-S2211124718315766-fx1.jpg%22%20width%3D%22800%22%20%2F%3E%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Problem%20statement%0A%0A%20%20%20%20The%20aim%20is%20to%20estimate%20the%20hidden%20state%20of%20a%20time%20varying%20system%20with%20the%20following%20properties%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20x_%7Bt%7D%20%3D%20F_t%20x_%7Bt-1%7D%20%2B%20B_t%20u_t%20%2B%20w_t%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%2C%0A%20%20%20%20-%20%5C(x_t%5C)%20is%20the%20hidden%20state%20at%20time%20%5C(t%5C)%0A%20%20%20%20-%20%5C(F_t%5C)%20is%20the%20state%20transition%20model%20which%20is%20applied%20to%20the%20previous%20state%20%5C(x_%7Bt-1%7D%5C)%0A%20%20%20%20-%20%5C(B_t%5C)%20is%20the%20control-input%20model%20which%20is%20applied%20to%20the%20control%20vector%20%5C(u_t%5C)%0A%20%20%20%20-%20%5C(w_t%5C)%20is%20the%20**process%20noise**%20which%20is%20assumed%20to%20be%20drawn%20from%20gaussian%20distribution%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20However%2C%20we%20do%20not%20have%20access%20to%20the%20hidden%20state%20directly.%20Instead%2C%20we%20have%20access%20to%20a%20noisy%20measurement%20%5C(z_t%5C)%20of%20the%20hidden%20state%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20z_t%20%3D%20H_t%20x_t%20%2B%20v_t%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%2C%0A%20%20%20%20-%20%5C(z_t%5C)%20is%20the%20measurement%20at%20time%20%5C(t%5C)%0A%20%20%20%20-%20%5C(H_t%5C)%20is%20the%20observation%20model%20which%20maps%20the%20true%20state%20space%20into%20the%20observed%20space%0A%20%20%20%20-%20%5C(v_t%5C)%20is%20the%20**measurement%20noise**%20which%20is%20also%20assumed%20to%20be%20drawn%20from%20gaussian%20distribution%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Kalman%20filter%20overview%0A%0A%20%20%20%20The%20Kalman%20filter%20can%20predict%20the%20hidden%20states%20for%20a%20system%20described%20using%20the%20two%20equations%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20x_%7Bt%7D%20%3D%20F_t%20x_%7Bt-1%7D%20%2B%20B_t%20u_t%20%2B%20w_t%0A%20%20%20%20%24%24%0A%0A%20%20%20%20%24%24%0A%20%20%20%20z_t%20%3D%20H_t%20x_t%20%2B%20v_t%0A%20%20%20%20%24%24%0A%0A%0A%20%20%20%20Based%20on%20these%2C%20the%20Kalman%20filter%20predicts%20the%20hidden%20state%20%5C(x_t%5C)%20and%20its%20covariance%20%5C(P_t%5C).%0A%0A%20%20%20%20%2F%2F%2F%20admonition%20%7C%20The%20catch%20is%20that%2C%20the%20Kalman%20filter%20assumes%20the%20following%20is%20known%3A%0A%20%20%20%20-%20The%20initial%20state%20%24x_0%24%20(reasonable)%0A%20%20%20%20-%20The%20control%20input%20%24u_t%24%20is%20known%20(reasonable)%0A%20%20%20%20-%20Transition%20matrixes%20%24F_t%24%2C%20%24B_t%24%2C%20%24H_t%24%20is%20known%20(reasonable%3F)%0A%20%20%20%20-%20The%20process%20error%20variance%20of%20w_t%20is%20known%20(might%20be%20hard%20to%20estimate%3F)%0A%20%20%20%20-%20The%20measurement%20error%20variance%20v_t%20is%20known%20(might%20be%20hard%20to%20estimate%3F)%0A%20%20%20%20%2F%2F%2F%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%20mo.md(r%22%22%22%0A%20%20%20%20It%20seems%20like%20a%20lot%20needs%20to%20known%20about%20the%20system%20before%20using%20a%20Kalman%20filter.%20So%2C%20why%20use%20it%20then%3F%0A%0A%20%20%20%20**The%20advantage%20is%20that%20Kalman%20filter%20is%20a%20recursive%20approximator%20%2F%20filter%20%2F%20predictor.**%20It%20runs%20in%20constant%20time%20and%20needs%20only%20the%20previous%20state%20to%20estimate%20the%20current%20state.%20This%20makes%20it%20suitable%20for%20real-time%20applications%20where%20data%20arrives%20sequentially.%0A%0A%20%20%20%20It%20runs%20in%20two%20steps%3A%0A%20%20%20%201.%20**Prediction%20step**%3A%20Based%20on%20the%20previous%20state%20estimate%20and%20the%20system%20model%2C%20predict%20the%20current%20state%20and%20its%20uncertainty.%0A%20%20%20%202.%20**Update%20step**%3A%20Incorporate%20the%20new%20measurement%20to%20refine%20the%20state%20estimate%20and%20reduce%20uncertainty.%0A%0A%20%20%20%20Because%20of%20this%2C%20it%20is%20often%20used%20in%20robotics%20and%20control%20applications.%20Additionally%2C%20multiple%20different%20types%20of%20measurements%20can%20be%20combined%20to%20improve%20the%20state%20estimate%2C%20making%20it%20useful%20in%20cases%20with%20different%20types%20of%20sensors.%0A%0A%20%20%20%20%2F%2F%2F%20attention%20%7C%20Assumptions%3A%0A%20%20%20%20Because%20Kalman%20filters%20rely%20on%20squared%20errors%20to%20estimate%20hidden%20states%2C%20they%20assume%20that%20the%20errors%20are%20normally%20distributed%20(similar%20to%20all%20least-square%20error%20minimization%20approaches).%20Therefore%2C%20Kalman%20filters%20work%20best%20when%20the%20noise%20in%20the%20system%20is%20Gaussian%20(it%20also%20works%20in%20some%20cases%20even%20if%20the%20noise%20is%20not%20gaussian).%0A%20%20%20%20%2F%2F%2F%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Train%20position%20estimation%20example%0A%0A%20%20%20%20%3Cimg%20src%3D%22public%2Fkalman_filter_01_01.png%22%20width%3D%22800%22%20%2F%3E%0A%0A%20%20%20%20The%20hidden%20state%20of%20the%20train%2C%20its%20position%20and%20velocity%2C%20needs%20to%20be%20known%20by%20the%20train%20operator.%0A%0A%20%20%20%20The%20available%20measurements%20are%3A%0A%20%20%20%201.%20velocity%20of%20the%20train%20(measured%20using%20a%20speedometer)%0A%20%20%20%202.%20position%20of%20the%20train%20(measured%20through%20a%20radio%20antenna%20on%20the%20roof)%0A%20%20%20%203.%20acceleration%2Fdeceleration%20of%20the%20train%20(inputs%20%24u_t%24)%0A%0A%20%20%20%20Based%20on%20the%20two%20noisy%20measurements%2C%20the%20position%20and%20velocity%20of%20the%20train%20needs%20to%20be%20estimated.%0A%0A%20%20%20%20Example%2C%20image%20inspired%20from%3A%20https%3A%2F%2Fcourses.physics.illinois.edu%2Fece420%2Fsp2019%2F7_UnderstandingKalmanFilter.pdf%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%20mo.md(rf%22%22%22%0A%20%20%20%20%23%23%23%20Train%20position%20estimation%20steps%0A%0A%20%20%20%20%3Cimg%20src%3D%22public%2Fkalman_filter_01_02.png%22%20width%3D%22800%22%20%2F%3E%0A%0A%20%20%20%20Images%20inspired%20from%3A%20https%3A%2F%2Fcourses.physics.illinois.edu%2Fece420%2Fsp2019%2F7_UnderstandingKalmanFilter.pdf%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20State%20estimation%20steps%0A%0A%20%20%20%20!%5Balt%5D(public%2Fkalman_filter_01_03.png)%0A%0A%20%20%20%20Image%20from%3A%20https%3A%2F%2Farxiv.org%2Fpdf%2F1710.04055%20(Fig.%204)%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Pictorial%20representation%0A%0A%20%20%20%20!%5Balt%5D(public%2Fkalman_filter_01_04.png)%0A%0A%20%20%20%20Image%20from%3A%20https%3A%2F%2Farxiv.org%2Fpdf%2F1710.04055%20(Fig.%205)%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Implementation%20the%20train%20example%0A%0A%20%20%20%20Hidden%20states%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20x_t%20%3D%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20p_t%5C%5C%0A%20%20%20%20v_t%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20State%20transition%20model%0A%0A%20%20%20%20%24%24%0A%20%20%20%20x_t%20%3D%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%201%20%26%20%5CDelta%20t%5C%5C%0A%20%20%20%200%20%26%201%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20p_%7Bt-1%7D%5C%5C%0A%20%20%20%20v_%7Bt-1%7D%0A%20%20%20%20%5Cend%7Bbmatrix%7D%20%2B%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%5Cfrac%7B%5CDelta%20t%5E2%7D%7B2%7D%5C%5C%0A%20%20%20%20%5CDelta%20t%0A%20%20%20%20%5Cend%7Bbmatrix%7D%20a_t%20%2B%20w_t%0A%20%20%20%20%24%24%0A%0A%20%20%20%20%24%24%0A%20%20%20%20x_t%20%3D%20F_t%20x_%7Bt-1%7D%20%2B%20B_t%20u_t%20%2B%20w_t%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Observation%20model%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20z_t%20%3D%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%201%20%26%200%5C%5C%0A%20%20%20%200%20%26%201%0A%20%20%20%20%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20p_%7Bt%7D%5C%5C%0A%20%20%20%20v_%7Bt%7D%0A%20%20%20%20%5Cend%7Bbmatrix%7D%20%2B%20v_t%0A%20%20%20%20%24%24%0A%0A%20%20%20%20%24%24%0A%20%20%20%20z_t%20%3D%20H_t%20x_t%20%2B%20v_t%0A%20%20%20%20%24%24%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%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Generate%20dataset%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo%2C%20np)%3A%0A%20%20%20%20process_cov_pos%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dnp.arange(1000%2C%20100000%2C%201000)%2C%0A%20%20%20%20%20%20%20%20value%3D5000.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Process%20noise%20variance%20(position)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20process_cov_vel%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dnp.arange(0%2C%2010.1%2C%200.1)%2C%0A%20%20%20%20%20%20%20%20value%3D0.5%2C%0A%20%20%20%20%20%20%20%20label%3D%22Process%20noise%20variance%20(velocity)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20measurement_cov_pos%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dnp.arange(10000%2C%20100000%2C%2010000)%2C%0A%20%20%20%20%20%20%20%20value%3D50000.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Measurement%20noise%20variance%20(position)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20measurement_cov_vel%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dnp.arange(0%2C%2020.1%2C%200.1)%2C%0A%20%20%20%20%20%20%20%20value%3D2.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Measurement%20noise%20variance%20(velocity)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20measurement_cov_pos%2C%0A%20%20%20%20%20%20%20%20measurement_cov_vel%2C%0A%20%20%20%20%20%20%20%20process_cov_pos%2C%0A%20%20%20%20%20%20%20%20process_cov_vel%2C%0A%20%20%20%20)%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20measurement_cov_pos%2C%0A%20%20%20%20measurement_cov_vel%2C%0A%20%20%20%20mo%2C%0A%20%20%20%20process_cov_pos%2C%0A%20%20%20%20process_cov_vel%2C%0A)%3A%0A%20%20%20%20mo.md(rf%22%22%22%0A%20%20%20%20%23%23%23%23%20Noise%20variances%0A%20%20%20%20-%20%7Bprocess_cov_pos%7D%0A%20%20%20%20-%20%7Bprocess_cov_vel%7D%0A%20%20%20%20-%20%7Bmeasurement_cov_pos%7D%0A%20%20%20%20-%20%7Bmeasurement_cov_vel%7D%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20measurement_cov_pos%2C%0A%20%20%20%20measurement_cov_vel%2C%0A%20%20%20%20np%2C%0A%20%20%20%20process_cov_pos%2C%0A%20%20%20%20process_cov_vel%2C%0A)%3A%0A%20%20%20%20%23%20Generate%20data%0A%20%20%20%20rng%20%3D%20np.random.default_rng(42)%0A%20%20%20%20num_steps%20%3D%20100%0A%20%20%20%20dt%20%3D%201.0%20%20%23%20time%20step%0A%0A%20%20%20%20%23%20True%20initial%20state%0A%20%20%20%20x0%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20%5B0%5D%2C%20%20%23%20initial%20position%0A%20%20%20%20%20%20%20%20%20%20%20%20%5B0%5D%2C%0A%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20)%20%20%23%20initial%20velocity%0A%20%20%20%20P0%20%3D%20np.array(%5B%5B0%2C%200%5D%2C%20%5B0%2C%200%5D%5D)%20%20%23%20initial%20covariance%0A%0A%20%20%20%20%23%20control%20input%20(acceleration)%0A%20%20%20%20acceleration%20%3D%20np.concat(%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20np.zeros(10)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%202%20*%20np.ones(20)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20np.ones(20)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20np.zeros(10)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20-np.ones(20)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20-2%20*%20np.ones(20)%2C%0A%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20)%0A%20%20%20%20%23%20print(acceleration)%0A%0A%20%20%20%20%23%20State%20transition%20model%0A%20%20%20%20F%20%3D%20np.array(%5B%5B1%2C%20dt%5D%2C%20%5B0%2C%201%5D%5D)%0A%20%20%20%20%23%20Control-input%20model%0A%20%20%20%20B%20%3D%20np.array(%5B%5B0.5%20*%20dt**2%5D%2C%20%5Bdt%5D%5D)%0A%0A%20%20%20%20%23%20True%20states%0A%20%20%20%20true_states%20%3D%20%5B%5D%0A%20%20%20%20x%20%3D%20x0%0A%20%20%20%20for%20a%20in%20acceleration%3A%0A%20%20%20%20%20%20%20%20x%20%3D%20F%20%40%20x%20%2B%20B%20*%20a%0A%20%20%20%20%20%20%20%20true_states.append(x.flatten())%0A%20%20%20%20true_states%20%3D%20np.array(true_states)%0A%0A%20%20%20%20%23%20%23%20Obtain%20noise%20variances%20from%20sliders%0A%20%20%20%20%23%20process_cov_pos%20%3D%201.0%20%20%23%20process%20noise%20variance%20for%20position%0A%20%20%20%20%23%20process_cov_vel%20%3D%200.5%20%20%23%20process%20noise%20variance%20for%20velocity%0A%20%20%20%20%23%20measurement_cov_pos%20%3D%205.0%20%20%23%20measurement%20noise%20variance%20for%20position%0A%20%20%20%20%23%20measurement_cov_vel%20%3D%202.0%20%20%23%20measurement%20noise%20variance%20for%20velocity%0A%0A%20%20%20%20%23%20Obtain%20noise%0A%20%20%20%20process_noise%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20rng.normal(0%2C%20np.sqrt(process_cov_pos.value)%2C%20size%3Dnum_steps)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20rng.normal(0%2C%20np.sqrt(process_cov_vel.value)%2C%20size%3Dnum_steps)%2C%0A%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20).transpose()%0A%0A%20%20%20%20measurement_noise%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20rng.normal(0%2C%20np.sqrt(measurement_cov_pos.value)%2C%20size%3Dnum_steps)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20rng.normal(0%2C%20np.sqrt(measurement_cov_vel.value)%2C%20size%3Dnum_steps)%2C%0A%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20).transpose()%0A%0A%20%20%20%20%23%20Noisy%20states%0A%20%20%20%20noisy_states%20%3D%20true_states%20%2B%20np.cumsum(process_noise%2C%20axis%3D0)%0A%0A%20%20%20%20%23%20Measurements%0A%20%20%20%20measurements%20%3D%20noisy_states%20%2B%20measurement_noise%0A%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20B%2C%0A%20%20%20%20%20%20%20%20F%2C%0A%20%20%20%20%20%20%20%20P0%2C%0A%20%20%20%20%20%20%20%20acceleration%2C%0A%20%20%20%20%20%20%20%20measurements%2C%0A%20%20%20%20%20%20%20%20noisy_states%2C%0A%20%20%20%20%20%20%20%20num_steps%2C%0A%20%20%20%20%20%20%20%20true_states%2C%0A%20%20%20%20%20%20%20%20x0%2C%0A%20%20%20%20)%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%23%23%20Plot%20generated%20data%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acceleration%2C%20measurements%2C%20noisy_states%2C%20plt%2C%20true_states)%3A%0A%20%20%20%20%23%20Plot%20data%0A%20%20%20%20plt.figure(figsize%3D(14%2C%2010))%0A%0A%20%20%20%20%23%20Plot%20Position%0A%20%20%20%20plt.subplot(3%2C%201%2C%201)%0A%20%20%20%20plt.plot(true_states%5B%3A%2C%200%5D%2C%20label%3D%22Position%20(noiseless%20model)%22%2C%20color%3D%22g%22)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20noisy_states%5B%3A%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Position%20(ground%20truth)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22r%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22--%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20measurements%5B%3A%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Measurements%20(process%20%2B%20observational%20noise)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22b%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22%3A%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.title(%22Position%20Estimation%22)%0A%20%20%20%20plt.xlabel(%22Time%20Steps%22)%0A%20%20%20%20plt.ylabel(%22Position%22)%0A%20%20%20%20plt.legend()%0A%0A%20%20%20%20%23%20Plot%20Velocity%0A%20%20%20%20plt.subplot(3%2C%201%2C%202)%0A%20%20%20%20plt.plot(true_states%5B%3A%2C%201%5D%2C%20label%3D%22Velocity%20(noiseless%20model)%22%2C%20color%3D%22g%22)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20noisy_states%5B%3A%2C%201%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Velocity%20(ground%20truth)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22r%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22--%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20measurements%5B%3A%2C%201%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Measurements%20(process%20%2B%20observational%20noise)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22b%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22%3A%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.title(%22Velocity%20Estimation%22)%0A%20%20%20%20plt.xlabel(%22Time%20Steps%22)%0A%20%20%20%20plt.ylabel(%22Velocity%22)%0A%20%20%20%20plt.legend()%0A%0A%20%20%20%20%23%20Plot%20Acceleration%0A%20%20%20%20plt.subplot(3%2C%201%2C%203)%0A%20%20%20%20plt.plot(acceleration%2C%20label%3D%22acceleration%22%2C%20color%3D%22b%22%2C%20linestyle%3D%22%3A%22)%0A%20%20%20%20plt.title(%22Acceleration%22)%0A%20%20%20%20plt.xlabel(%22Time%20Steps%22)%0A%20%20%20%20plt.ylabel(%22Acceleration%22)%0A%20%20%20%20plt.legend()%0A%0A%20%20%20%20plt.tight_layout()%0A%20%20%20%20plt.gca()%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%23%20Run%20Kalman%20filter%20on%20generated%20data%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20class%20KalmanFilter%3A%0A%20%20%20%20%20%20%20%20def%20__init__(self%2C%20F%2C%20B%2C%20H%2C%20Q%2C%20R%2C%20x0%2C%20P0)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20self.F%20%3D%20F%20%20%23%20State%20transition%20model%0A%20%20%20%20%20%20%20%20%20%20%20%20self.B%20%3D%20B%20%20%23%20Control-input%20model%0A%20%20%20%20%20%20%20%20%20%20%20%20self.H%20%3D%20H%20%20%23%20Observation%20model%0A%20%20%20%20%20%20%20%20%20%20%20%20self.Q%20%3D%20Q%20%20%23%20Process%20noise%20covariance%0A%20%20%20%20%20%20%20%20%20%20%20%20self.R%20%3D%20R%20%20%23%20Measurement%20noise%20covariance%0A%20%20%20%20%20%20%20%20%20%20%20%20self.x%20%3D%20x0%20%20%23%20Initial%20state%20estimate%0A%20%20%20%20%20%20%20%20%20%20%20%20self.x_pred%20%3D%20None%20%20%23%20Predicted%20state%20estimate%0A%20%20%20%20%20%20%20%20%20%20%20%20self.P%20%3D%20P0%20%20%23%20Initial%20covariance%20estimate%0A%20%20%20%20%20%20%20%20%20%20%20%20self.P_pred%20%3D%20None%20%20%23%20Predicted%20covariance%20estimate%0A%0A%20%20%20%20%20%20%20%20def%20predict(self%2C%20u)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%23%20Predict%20the%20next%20state%0A%20%20%20%20%20%20%20%20%20%20%20%20self.x_pred%20%3D%20self.F%20%40%20self.x%20%2B%20self.B%20%40%20u%0A%20%20%20%20%20%20%20%20%20%20%20%20self.P_pred%20%3D%20self.F%20%40%20self.P%20%40%20self.F.T%20%2B%20self.Q%0A%0A%20%20%20%20%20%20%20%20def%20update(self%2C%20z)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%23%20Update%20the%20state%20with%20measurement%20z%0A%20%20%20%20%20%20%20%20%20%20%20%20y%20%3D%20z%20-%20self.H%20%40%20self.x_pred%20%20%23%20Measurement%20residual%0A%20%20%20%20%20%20%20%20%20%20%20%20S%20%3D%20self.H%20%40%20self.P_pred%20%40%20self.H.T%20%2B%20self.R%20%20%23%20Residual%20covariance%0A%20%20%20%20%20%20%20%20%20%20%20%20K%20%3D%20self.P_pred%20%40%20self.H.T%20%40%20np.linalg.inv(S)%20%20%23%20Kalman%20gain%0A%0A%20%20%20%20%20%20%20%20%20%20%20%20self.x%20%3D%20self.x%20%2B%20K%20%40%20y%0A%20%20%20%20%20%20%20%20%20%20%20%20I%20%3D%20np.eye(self.P.shape%5B0%5D)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.P%20%3D%20(I%20-%20K%20%40%20self.H)%20%40%20self.P_pred%0A%20%20%20%20return%20(KalmanFilter%2C)%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%23%23%20Guess%20noise%20covariance%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20measurement_cov_pos%2C%0A%20%20%20%20measurement_cov_vel%2C%0A%20%20%20%20mo%2C%0A%20%20%20%20process_cov_pos%2C%0A%20%20%20%20process_cov_vel%2C%0A)%3A%0A%20%20%20%20use_data_cov%20%3D%20mo.ui.checkbox(%0A%20%20%20%20%20%20%20%20label%3D%22Use%20data%20as%20guesses%20for%20Kalman%20filter%3F%22%2C%0A%20%20%20%20%20%20%20%20value%3DTrue%2C%0A%20%20%20%20)%0A%0A%20%20%20%20g_process_cov_pos%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dprocess_cov_pos.steps%2C%0A%20%20%20%20%20%20%20%20value%3D2000.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Guess%20of%20process%20noise%20variance%20(position)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20g_process_cov_vel%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dprocess_cov_vel.steps%2C%0A%20%20%20%20%20%20%20%20value%3D1.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Guess%20of%20process%20noise%20variance%20(velocity)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20g_measurement_cov_pos%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dmeasurement_cov_pos.steps%2C%0A%20%20%20%20%20%20%20%20value%3D70000.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Guess%20of%20measurement%20noise%20variance%20(position)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20g_measurement_cov_vel%20%3D%20mo.ui.slider(%0A%20%20%20%20%20%20%20%20steps%3Dmeasurement_cov_vel.steps%2C%0A%20%20%20%20%20%20%20%20value%3D4.0%2C%0A%20%20%20%20%20%20%20%20label%3D%22Guess%20of%20measurement%20noise%20variance%20(velocity)%3A%22%2C%0A%20%20%20%20%20%20%20%20show_value%3DTrue%2C%0A%20%20%20%20)%0A%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20g_measurement_cov_pos%2C%0A%20%20%20%20%20%20%20%20g_measurement_cov_vel%2C%0A%20%20%20%20%20%20%20%20g_process_cov_pos%2C%0A%20%20%20%20%20%20%20%20g_process_cov_vel%2C%0A%20%20%20%20%20%20%20%20use_data_cov%2C%0A%20%20%20%20)%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20g_measurement_cov_pos%2C%0A%20%20%20%20g_measurement_cov_vel%2C%0A%20%20%20%20g_process_cov_pos%2C%0A%20%20%20%20g_process_cov_vel%2C%0A%20%20%20%20mo%2C%0A%20%20%20%20use_data_cov%2C%0A)%3A%0A%20%20%20%20mo.md(rf%22%22%22%0A%20%20%20%20%7Buse_data_cov%7D%0A%0A%20%20%20%20Covariance%20guesses%3A%0A%20%20%20%20-%20%7Bg_process_cov_pos%7D%0A%20%20%20%20-%20%7Bg_process_cov_vel%7D%0A%20%20%20%20-%20%7Bg_measurement_cov_pos%7D%0A%20%20%20%20-%20%7Bg_measurement_cov_vel%7D%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20B%2C%0A%20%20%20%20F%2C%0A%20%20%20%20KalmanFilter%2C%0A%20%20%20%20P0%2C%0A%20%20%20%20g_measurement_cov_pos%2C%0A%20%20%20%20g_measurement_cov_vel%2C%0A%20%20%20%20g_process_cov_pos%2C%0A%20%20%20%20g_process_cov_vel%2C%0A%20%20%20%20measurement_cov_pos%2C%0A%20%20%20%20measurement_cov_vel%2C%0A%20%20%20%20np%2C%0A%20%20%20%20process_cov_pos%2C%0A%20%20%20%20process_cov_vel%2C%0A%20%20%20%20use_data_cov%2C%0A%20%20%20%20x0%2C%0A)%3A%0A%20%20%20%20%23%20Obtain%20covariance%20guesses%0A%20%20%20%20if%20use_data_cov.value%3A%0A%20%20%20%20%20%20%20%20train_kalman_filter%20%3D%20KalmanFilter(%0A%20%20%20%20%20%20%20%20%20%20%20%20F%3DF%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20B%3DB%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%23%20H%3Dnp.array(%5B%5B1%2C%200%5D%2C%20%5B0%2C%200%5D%5D)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20H%3Dnp.eye(2)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20Q%3Dnp.array(%5B%5Bprocess_cov_pos.value%2C%200%5D%2C%20%5B0%2C%20process_cov_vel.value%5D%5D)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20R%3Dnp.array(%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5Bmeasurement_cov_pos.value%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5B0%2C%20measurement_cov_vel.value%5D%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20x0%3Dx0%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20P0%3DP0%2C%0A%20%20%20%20%20%20%20%20)%0A%0A%20%20%20%20else%3A%0A%20%20%20%20%20%20%20%20train_kalman_filter%20%3D%20KalmanFilter(%0A%20%20%20%20%20%20%20%20%20%20%20%20F%3DF%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20B%3DB%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%23%20H%3Dnp.array(%5B%5B1%2C%200%5D%2C%20%5B0%2C%200%5D%5D)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20H%3Dnp.eye(2)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20Q%3Dnp.array(%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5B%5Bg_process_cov_pos.value%2C%200%5D%2C%20%5B0%2C%20g_process_cov_vel.value%5D%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20R%3Dnp.array(%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5Bg_measurement_cov_pos.value%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5B0%2C%20g_measurement_cov_vel.value%5D%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20x0%3Dx0%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20P0%3DP0%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20return%20(train_kalman_filter%2C)%0A%0A%0A%40app.cell%0Adef%20_(acceleration%2C%20measurements%2C%20np%2C%20num_steps%2C%20train_kalman_filter)%3A%0A%20%20%20%20%23%20Estimate%20states%0A%20%20%20%20estimated_states%20%3D%20%5B%5D%0A%20%20%20%20for%20j%20in%20range(num_steps)%3A%0A%20%20%20%20%20%20%20%20u%20%3D%20np.array(%5B%5Bacceleration%5Bj%5D%5D%5D)%20%20%23%20control%20input%20(acceleration)%0A%20%20%20%20%20%20%20%20z%20%3D%20measurements%5Bj%5D.reshape(-1%2C%201)%20%20%23%20measurement%0A%0A%20%20%20%20%20%20%20%20train_kalman_filter.predict(u)%0A%20%20%20%20%20%20%20%20train_kalman_filter.update(z)%0A%0A%20%20%20%20%20%20%20%20estimated_states.append(train_kalman_filter.x.flatten())%0A%20%20%20%20estimated_states%20%3D%20np.array(estimated_states)%0A%20%20%20%20return%20(estimated_states%2C)%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%23%23%20Plot%20estimation%20states%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(estimated_states%2C%20measurements%2C%20noisy_states%2C%20plt%2C%20true_states)%3A%0A%20%20%20%20%23%20Plot%20estimated%20states%0A%0A%20%20%20%20plt.figure(figsize%3D(14%2C%2010))%0A%20%20%20%20%23%20Plot%20Position%0A%20%20%20%20plt.subplot(2%2C%201%2C%201)%0A%20%20%20%20plt.plot(true_states%5B%3A%2C%200%5D%2C%20label%3D%22Position%20(noiseless%20model)%22%2C%20color%3D%22g%22)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20noisy_states%5B%3A%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Position%20(ground%20truth)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22r%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22--%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20measurements%5B%3A%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Measurements%20(process%20%2B%20observational%20noise)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22b%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22%3A%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20estimated_states%5B%3A%2C%200%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Estimated%20Position%20(Kalman%20filter)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22orange%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22-.%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.title(%22Position%20Estimation%20with%20Kalman%20Filter%22)%0A%20%20%20%20plt.xlabel(%22Time%20Steps%22)%0A%20%20%20%20plt.ylabel(%22Position%22)%0A%20%20%20%20plt.legend()%0A%0A%20%20%20%20%23%20Plot%20Velocity%0A%20%20%20%20plt.subplot(2%2C%201%2C%202)%0A%20%20%20%20plt.plot(true_states%5B%3A%2C%201%5D%2C%20label%3D%22Velocity%20(noiseless%20model)%22%2C%20color%3D%22g%22)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20noisy_states%5B%3A%2C%201%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Velocity%20(ground%20truth)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22r%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22--%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20measurements%5B%3A%2C%201%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Measurements%20(process%20%2B%20observational%20noise)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22b%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22%3A%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.plot(%0A%20%20%20%20%20%20%20%20estimated_states%5B%3A%2C%201%5D%2C%0A%20%20%20%20%20%20%20%20label%3D%22Estimated%20Velocity%20(Kalman%20filter)%22%2C%0A%20%20%20%20%20%20%20%20color%3D%22orange%22%2C%0A%20%20%20%20%20%20%20%20linestyle%3D%22-.%22%2C%0A%20%20%20%20)%0A%20%20%20%20plt.title(%22Velocity%20Estimation%20with%20Kalman%20Filter%22)%0A%20%20%20%20plt.xlabel(%22Time%20Steps%22)%0A%20%20%20%20plt.ylabel(%22Velocity%22)%0A%20%20%20%20plt.legend()%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