kalman

import%20marimo%0A%0A__generated_with%20%3D%20%220.24.0%22%0Aapp%20%3D%20marimo.App()%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%0A%20%20%20%20return%20(mo%2C)%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%20%23%20%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%0A%0A%20%20%20%20%E3%83%8E%E3%82%A4%E3%82%BA%E3%81%AE%E4%B9%97%E3%81%A3%E3%81%9F%E8%A6%B3%E6%B8%AC%E3%81%8B%E3%82%89%E7%8A%B6%E6%85%8B%E3%82%92%E6%8E%A8%E5%AE%9A%E3%81%99%E3%82%8B%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%82%92%E3%80%81%E3%81%BE%E3%81%9A%E8%87%AA%E5%89%8D%E5%AE%9F%E8%A3%85%E3%81%A7%0A%20%20%20%20%E4%BB%95%E7%B5%84%E3%81%BF%E3%82%92%E8%BF%BD%E3%81%84%E3%80%81%E6%AC%A1%E3%81%AB%E9%9D%9E%E7%B7%9A%E5%BD%A2%E7%B3%BB%E3%81%B8%E6%8B%A1%E5%BC%B5%E3%81%97%E3%81%9F%20EKF%EF%BC%88%E6%8B%A1%E5%BC%B5%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%EF%BC%89%E3%82%92%E6%89%B1%E3%81%86%E3%80%82%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%20%23%23%20%E3%81%93%E3%81%AE%20notebook%20%E3%81%AE%E5%85%B1%E9%80%9A%E9%83%A8%E5%93%81%0A%0A%20%20%20%20%E6%AC%A1%E3%81%AE%E3%82%BB%E3%83%AB%E3%81%AF%E3%80%81%E4%BB%A5%E9%99%8D%E3%81%AE%E3%81%99%E3%81%B9%E3%81%A6%E3%81%AE%E3%82%A2%E3%83%8B%E3%83%A1%E3%83%BC%E3%82%B7%E3%83%A7%E3%83%B3%E3%81%8C%E5%85%B1%E6%9C%89%E3%81%99%E3%82%8B%E5%87%BA%E5%8A%9B%E5%85%88%E3%81%A8%E3%83%98%E3%83%AB%E3%83%91%E3%83%BC%E3%82%92%E7%94%A8%E6%84%8F%E3%81%99%E3%82%8B%E3%80%82%0A%0A%20%20%20%20-%20%60artifacts_dir%60%3A%20GIF%20%E3%81%AE%E4%BF%9D%E5%AD%98%E5%85%88%20%60notebooks%2Falgorithm%2F_generated%2Fkalman%2F%60%E3%80%82%0A%20%20%20%20%20%20Git%20%E3%81%AE%E8%BF%BD%E8%B7%A1%E5%AF%BE%E8%B1%A1%E5%A4%96%E3%81%AA%E3%81%AE%E3%81%A7%E3%80%81%E5%AE%9F%E8%A1%8C%E3%81%99%E3%82%8B%E3%81%9F%E3%81%B3%E3%81%AB%E5%86%8D%E7%94%9F%E6%88%90%E3%81%95%E3%82%8C%E3%82%8B%E3%80%82%0A%20%20%20%20-%20%60gif_image(path%2C%20alt)%60%3A%20GIF%20%E3%82%92%20base64%20%E3%81%AE%20data%20URI%20%E3%81%AB%E5%A4%89%E6%8F%9B%E3%81%97%E3%81%A6%20%60mo.image%60%20%E3%81%A7%0A%20%20%20%20%20%20%E5%9F%8B%E3%82%81%E8%BE%BC%E3%82%80%E3%80%82%E9%9D%99%E7%9A%84%20HTML%20%E3%81%B8%E6%9B%B8%E3%81%8D%E5%87%BA%E3%81%97%E3%81%9F%E3%81%A8%E3%81%8D%E3%82%82%E5%A4%96%E9%83%A8%E3%83%95%E3%82%A1%E3%82%A4%E3%83%AB%E3%82%92%E5%8F%82%E7%85%A7%E3%81%9B%E3%81%9A%E3%81%AB%E5%86%8D%E7%94%9F%E3%81%A7%E3%81%8D%E3%82%8B%E3%80%82%0A%20%20%20%20-%20%60save_gif(fig%2C%20update%2C%20frames%2C%20path%2C%20interval)%60%3A%20%60FuncAnimation%60%20%E3%82%92%20GIF%20%E3%81%A8%E3%81%97%E3%81%A6%0A%20%20%20%20%20%20%E4%BF%9D%E5%AD%98%E3%81%99%E3%82%8B%E3%80%82%60frames%60%20%E3%81%AB%E3%81%AF%E3%82%B3%E3%83%9E%E6%95%B0%EF%BC%88%60int%60%EF%BC%89%E3%81%8B%E3%82%B3%E3%83%9E%E7%95%AA%E5%8F%B7%E3%81%AE%E4%B8%A6%E3%81%B3%EF%BC%88%60range%60%20%E3%81%AA%E3%81%A9%EF%BC%89%E3%82%92%E6%B8%A1%E3%81%99%E3%80%82%0A%20%20%20%20%20%20%60interval%60%20%E3%81%AF%201%20%E3%82%B3%E3%83%9E%E3%81%AE%E8%A1%A8%E7%A4%BA%E6%99%82%E9%96%93%EF%BC%88%E3%83%9F%E3%83%AA%E7%A7%92%EF%BC%89%E3%81%A7%E3%81%82%E3%82%8B%E3%80%82GIF%20%E3%81%AF%E8%A1%A8%E7%A4%BA%E6%99%82%E9%96%93%E3%82%92%201%2F100%20%E7%A7%92%E5%8D%98%E4%BD%8D%E3%81%A7%0A%20%20%20%20%20%20%E3%81%97%E3%81%8B%E4%BF%9D%E6%8C%81%E3%81%A7%E3%81%8D%E3%81%AA%E3%81%84%E3%81%9F%E3%82%81%E3%80%81%E8%A6%81%E6%B1%82%E3%81%97%E3%81%9F%E5%80%A4%E3%81%8C%E3%81%9D%E3%81%AE%E3%81%BE%E3%81%BE%E4%BD%BF%E3%82%8F%E3%82%8C%E3%82%8B%E3%81%A8%E3%81%AF%E9%99%90%E3%82%89%E3%81%AA%E3%81%84%E3%80%82%E4%BF%9D%E5%AD%98%E5%BE%8C%E3%81%AB%0A%20%20%20%20%20%20GIF%20%E3%82%92%E8%AA%AD%E3%81%BF%E7%9B%B4%E3%81%97%E3%80%81%E5%AE%9F%E9%9A%9B%E3%81%AE%E9%96%93%E9%9A%94%E3%83%BB%E5%86%8D%E7%94%9F%E6%99%82%E9%96%93%E3%83%BB%E3%83%95%E3%82%A1%E3%82%A4%E3%83%AB%E3%82%B5%E3%82%A4%E3%82%BA%E3%82%92%E6%B8%AC%E3%81%A3%E3%81%A6%E8%BF%94%E3%81%99%E3%80%82%0A%0A%20%20%20%20%E4%BB%A5%E9%99%8D%E3%81%AE%20GIF%20%E3%81%AF%E3%81%99%E3%81%B9%E3%81%A6%20%60interval%3D100%60%EF%BC%8810%20%E3%82%B3%E3%83%9E%2F%E7%A7%92%EF%BC%89%E3%81%AB%E6%8F%83%E3%81%88%E3%81%A6%E3%81%82%E3%82%8B%E3%80%82%E6%8C%99%E5%8B%95%E3%82%92%E7%9B%AE%E3%81%A7%E8%BF%BD%E3%81%86%0A%20%20%20%20%E3%81%AB%E3%81%AF%2020%20%E3%82%B3%E3%83%9E%2F%E7%A7%92%E3%81%A7%E3%81%AF%E9%80%9F%E3%81%99%E3%81%8E%E3%82%8B%E3%81%9F%E3%82%81%E3%80%81%E6%84%8F%E5%9B%B3%E7%9A%84%E3%81%AB%E8%90%BD%E3%81%A8%E3%81%97%E3%81%A6%E3%81%82%E3%82%8B%E3%80%82%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%20import%20base64%0A%20%20%20%20from%20pathlib%20import%20Path%0A%0A%20%20%20%20import%20matplotlib.animation%20as%20animation%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt%0A%20%20%20%20from%20PIL%20import%20Image%0A%0A%20%20%20%20artifacts_dir%20%3D%20Path(__file__).resolve().parent%20%2F%20%22_generated%22%20%2F%20%22kalman%22%0A%20%20%20%20artifacts_dir.mkdir(parents%3DTrue%2C%20exist_ok%3DTrue)%0A%0A%20%20%20%20def%20gif_image(path%3A%20Path%2C%20alt%3A%20str)%3A%0A%20%20%20%20%20%20%20%20encoded%20%3D%20base64.b64encode(path.read_bytes()).decode()%0A%20%20%20%20%20%20%20%20return%20mo.image(f%22data%3Aimage%2Fgif%3Bbase64%2C%7Bencoded%7D%22%2C%20alt%3Dalt)%0A%0A%20%20%20%20def%20save_gif(fig%2C%20update%2C%20frames%2C%20path%3A%20Path%2C%20interval%3A%20int)%20-%3E%20str%3A%0A%20%20%20%20%20%20%20%20frame_list%20%3D%20list(range(frames))%20if%20isinstance(frames%2C%20int)%20else%20list(frames)%0A%20%20%20%20%20%20%20%20anim%20%3D%20animation.FuncAnimation(fig%2C%20update%2C%20frames%3Dframe_list%2C%20interval%3Dinterval)%0A%20%20%20%20%20%20%20%20anim.save(path%2C%20writer%3D%22pillow%22)%0A%20%20%20%20%20%20%20%20plt.close(fig)%0A%20%20%20%20%20%20%20%20%23%20GIF%20%E3%81%AF%E9%96%93%E9%9A%94%E3%82%92%201%2F100%20%E7%A7%92%E5%8D%98%E4%BD%8D%E3%81%A7%E3%81%97%E3%81%8B%E4%BF%9D%E6%8C%81%E3%81%A7%E3%81%8D%E3%81%AA%E3%81%84%E3%81%AE%E3%81%A7%E3%80%81%E4%BF%9D%E5%AD%98%E5%BE%8C%E3%81%AB%E5%AE%9F%E6%B8%AC%E3%81%97%E3%81%A6%E5%A0%B1%E5%91%8A%E3%81%99%E3%82%8B%E3%80%82%0A%20%20%20%20%20%20%20%20with%20Image.open(path)%20as%20gif%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20durations%20%3D%20%5B%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20index%20in%20range(gif.n_frames)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20gif.seek(index)%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20durations.append(gif.info.get(%22duration%22%2C%200))%0A%20%20%20%20%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%7Bpath.name%7D%3A%20%7Blen(frame_list)%7D%20frames%2C%20%7Bdurations%5B0%5D%7D%20ms%2Fframe%2C%20%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%7Bsum(durations)%20%2F%201000%3A.1f%7D%20s%2C%20%7Bpath.stat().st_size%20%2F%201024%3A.0f%7D%20KiB%22%0A%20%20%20%20%20%20%20%20)%0A%0A%20%20%20%20return%20artifacts_dir%2C%20gif_image%2C%20save_gif%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%20%23%23%20Kalman%20Filters%20from%20scratch%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%20%23%23%23%20%E4%BD%BF%E7%94%A8%E3%81%99%E3%82%8B%E9%81%8B%E5%8B%95%E3%83%A2%E3%83%87%E3%83%AB%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%20%E5%86%86%E9%81%8B%E5%8B%95%E3%83%A2%E3%83%87%E3%83%AB%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x%20%5C%5C%20y%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20%5Ccos%5Ctheta%20%5C%5C%0A%20%20%20%20%5Csin%5Ctheta%0A%20%20%20%20%5Cend%7Bpmatrix%7D%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_()%3A%0A%20%20%20%20import%20numpy%20as%20np%0A%0A%20%20%20%20np.random.seed(42)%0A%20%20%20%20dt%20%3D%202%20*%20np.pi%20%2F%20100%0A%20%20%20%20_t%20%3D%20np.arange(0%2C%202%20*%20np.pi%2C%20dt)%0A%20%20%20%20x%20%3D%20np.sin(_t)%0A%20%20%20%20y%20%3D%20np.cos(_t)%0A%20%20%20%20points%20%3D%20np.vstack((x%2C%20y)).T%0A%20%20%20%20return%20dt%2C%20np%2C%20points%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%20%E9%85%8D%E8%89%B2%E3%82%92%E5%AE%9A%E7%BE%A9%E3%80%82%0A%20%20%20%20%E8%A9%B3%E7%B4%B0%E3%81%AF%20%5Bmatplotlib%20-%20List%20of%20named%20colors%5D(https%3A%2F%2Fmatplotlib.org%2Fstable%2Fgallery%2Fcolor%2Fnamed_colors.html%23tableau-palette)%20%E3%82%92%E5%8F%82%E7%85%A7%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20from%20matplotlib.colors%20import%20TABLEAU_COLORS%0A%0A%20%20%20%20blue%20%3D%20TABLEAU_COLORS%5B%22tab%3Ablue%22%5D%0A%20%20%20%20orange%20%3D%20TABLEAU_COLORS%5B%22tab%3Aorange%22%5D%0A%20%20%20%20green%20%3D%20TABLEAU_COLORS%5B%22tab%3Agreen%22%5D%0A%20%20%20%20red%20%3D%20TABLEAU_COLORS%5B%22tab%3Ared%22%5D%0A%20%20%20%20return%20blue%2C%20green%2C%20orange%2C%20red%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%20%E8%BB%8C%E8%B7%A1%E3%81%AE%E3%82%A2%E3%83%8B%E3%83%A1%E3%83%BC%E3%82%B7%E3%83%A7%E3%83%B3%E4%BD%9C%E6%88%90%E3%80%82%E3%81%BE%E3%81%9A%E3%81%AF%E3%83%91%E3%83%AC%E3%83%83%E3%83%88%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt_circle%0A%0A%20%20%20%20circle_fig%2C%20circle_ax%20%3D%20plt_circle.subplots()%0A%20%20%20%20circle_ax.set_aspect('equal')%0A%20%20%20%20circle_ax.set_xlim(-1.3%2C%201.3)%0A%20%20%20%20circle_ax.set_ylim(-1.3%2C%201.3)%0A%20%20%20%20return%20circle_ax%2C%20circle_fig%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%20%E3%82%A2%E3%83%8B%E3%83%A1%E3%83%BC%E3%82%B7%E3%83%A7%E3%83%B3%E6%8F%8F%E7%94%BB%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20artifacts_dir%2C%0A%20%20%20%20blue%2C%0A%20%20%20%20circle_ax%2C%0A%20%20%20%20circle_fig%2C%0A%20%20%20%20gif_image%2C%0A%20%20%20%20orange%2C%0A%20%20%20%20points%2C%0A%20%20%20%20save_gif%2C%0A)%3A%0A%20%20%20%20circle_traj_plt%2C%20%3D%20circle_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dblue%2C%20label%3D'Trajectory')%0A%20%20%20%20circle_cur_pos_plt%2C%20%3D%20circle_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dorange%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'Current%20Position')%0A%20%20%20%20circle_ax.legend(loc%3D'upper%20right'%2C%20fontsize%3D'x-small')%0A%0A%20%20%20%20def%20circle_anim_callback(i)%3A%0A%20%20%20%20%20%20%20%20circle_ax.set_title(f'Frame%20%7Bi%7D')%0A%20%20%20%20%20%20%20%20circle_traj_plt.set_data(points%5B%3Ai%2C%200%5D%2C%20points%5B%3Ai%2C%201%5D)%0A%20%20%20%20%20%20%20%20circle_cur_pos_plt.set_data(%5Bpoints%5Bi%2C%200%5D%5D%2C%20%5Bpoints%5Bi%2C%201%5D%5D)%0A%0A%20%20%20%20circle_gif_path%20%3D%20artifacts_dir%20%2F%20%22circle_motion.gif%22%0A%20%20%20%20print(save_gif(circle_fig%2C%20circle_anim_callback%2C%20points.shape%5B0%5D%2C%20circle_gif_path%2C%20100))%0A%20%20%20%20move_animation%20%3D%20gif_image(circle_gif_path%2C%20%22%E5%86%86%E9%81%8B%E5%8B%95%E3%81%99%E3%82%8B%E7%9C%9F%E5%80%A4%E3%81%AE%E8%BB%8C%E8%B7%A1%22)%0A%20%20%20%20return%20(move_animation%2C)%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%20%E5%86%8D%E7%94%9F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(move_animation)%3A%0A%20%20%20%20move_animation%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%20%23%23%23%20EKF%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%20%E3%81%BE%E3%81%9A%E3%81%AF%E6%AD%A3%E8%A6%8F%E5%88%86%E5%B8%83%E3%83%8E%E3%82%A4%E3%82%BA%E3%82%92%E4%B8%8E%E3%81%88%E3%81%A6%E7%96%91%E4%BC%BC%E8%A6%B3%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%82%92%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20points)%3A%0A%20%20%20%20noise_sigma%20%3D%200.4%0A%20%20%20%20circle_obs_rng%20%3D%20np.random.default_rng(43)%0A%20%20%20%20circle_obs_noise%20%3D%20circle_obs_rng.normal(0%2C%20noise_sigma%20**%202%2C%20(points.shape%5B0%5D%2C%202))%0A%20%20%20%20points_obs%20%3D%20points%20%2B%20circle_obs_noise%0A%20%20%20%20return%20noise_sigma%2C%20points_obs%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%20EKF%20%E3%81%AF%E9%9D%9E%E7%B7%9A%E5%BD%A2%E9%81%8B%E5%8B%95%E3%83%A2%E3%83%87%E3%83%AB%E3%82%92%E7%B7%9A%E5%BD%A2%E8%BF%91%E4%BC%BC%E3%81%97%E3%81%A6%20Kalman%20Filter%20%E3%82%92%E9%81%A9%E7%94%A8%E3%81%99%E3%82%8B%E3%81%A0%E3%81%91%E3%80%82%0A%20%20%20%20Kalman%20Filter%20%E3%81%AE%E5%85%AC%E5%BC%8F%E3%81%AF%5B%E3%81%93%E3%81%93%5D(https%3A%2F%2Fja.wikipedia.org%2Fwiki%2F%25E3%2582%25AB%25E3%2583%25AB%25E3%2583%259E%25E3%2583%25B3%25E3%2583%2595%25E3%2582%25A3%25E3%2583%25AB%25E3%2582%25BF%25E3%2583%25BC)%E3%82%92%E5%8F%82%E7%85%A7%E3%80%82%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%20%E4%BB%8A%E5%9B%9E%E3%81%AE%20EKF%20%E3%81%A7%E3%81%AE%E9%81%8B%E5%8B%95%E3%83%A2%E3%83%87%E3%83%AB%E3%81%AE%E8%BF%91%E4%BC%BC%E5%BC%8F%E3%81%AF%E6%AC%A1%E3%81%AE%E9%80%9A%E3%82%8A%E3%80%82%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x_%7Bk%2B1%7D%20%5C%5C%20y_%7Bk%2B1%7D%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20%5Ccos%5Ctheta_%7Bk%2B1%7D%20%5C%5C%20%5Csin%5Ctheta_%7Bk%2B1%7D%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20%5Ccos(%5Ctheta_k%2B%5Cdelta%5Ctheta)%20%5C%5C%20%5Csin(%5Ctheta_k%2B%5Cdelta%5Ctheta)%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%201%20%26%20-%5Cdelta%5Ctheta%20%5C%5C%0A%20%20%20%20%5Cdelta%5Ctheta%20%26%201%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x_k%20%5C%5C%20y_k%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%2B%5Cfrac%7B1%7D%7B2%7D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20%20%5Ccos%5Ctheta_0%5Cdelta%5Ctheta%5E2%20%5C%5C%0A%20%20%20%20%20%5Csin%5Ctheta_0%5Cdelta%5Ctheta%5E2%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%2B%5Cmathcal%7BO%7D(%5Cdelta%5Ctheta%5E3)%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%E8%AA%A4%E5%B7%AE%E7%B5%B6%E5%AF%BE%E5%80%A4%E3%81%AE%E4%B8%8A%E9%99%90%E5%80%A4%E3%82%92%E7%94%A8%E3%81%84%E3%81%A6%E6%AC%A1%E3%81%AE%E8%BF%91%E4%BC%BC%E3%82%92%E3%81%99%E3%82%8B%E3%80%82%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x_%7Bk%2B1%7D%20%5C%5C%20y_%7Bk%2B1%7D%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%5Csimeq%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%201%20%26%20-%5Cdelta%5Ctheta%20%5C%5C%0A%20%20%20%20%5Cdelta%5Ctheta%20%26%201%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x_k%20%5C%5C%20y_k%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%2Bw_k%2C%5Cquad%0A%20%20%20%20w_k%5Csim%20N%5Cleft(0%2C%5Cfrac%7B1%7D%7B2%7D%5Cdelta%5Ctheta%5E2I%5Cright)%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%E8%A6%B3%E6%B8%AC%E3%83%A2%E3%83%87%E3%83%AB%E3%81%AF%24(x%2Cy)%24%E5%A4%89%E6%95%B0%E3%81%AE%E5%A4%9A%E5%A4%89%E9%87%8F%E6%AD%A3%E8%A6%8F%E5%88%86%E5%B8%83%E3%81%A8%E3%81%99%E3%82%8B%E3%80%82%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x_k'%20%5C%5C%20y_k'%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20x_k%20%5C%5C%20y_k%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%2Bv_k%2C%5Cquad%0A%20%20%20%20v_k%5Csim%20N(0%2C%5Csigma%5E2%20I)%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%E5%B7%AE%E5%88%86%E5%8C%96%E3%81%AE%E5%88%BB%E3%81%BF%E5%B9%85%EF%BC%88FPS%EF%BC%89%E3%81%AF%E7%9F%AD%E3%81%84%E3%81%BB%E3%81%A9%E6%8B%A1%E5%BC%B5%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%81%AE%E7%B2%BE%E5%BA%A6%E3%81%AF%E8%89%AF%E3%81%8F%E3%81%AA%E3%82%8B%E3%80%82%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%20EKF%3A%0A%0A%20%20%20%20%20%20%20%20def%20__init__(self%2C%20state%3A%20np.ndarray%2C%20state_error%3A%20np.ndarray%2C%20pred_mat%3A%20np.ndarray%2C%20pred_error%3A%20np.ndarray%2C%20obs_mat%3A%20np.ndarray%2C%20obs_error%3A%20np.ndarray)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state%20%3D%20state%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state_error%20%3D%20state_error%0A%20%20%20%20%20%20%20%20%20%20%20%20self.pred_error%20%3D%20pred_error%0A%20%20%20%20%20%20%20%20%20%20%20%20self.obs_error%20%3D%20obs_error%0A%20%20%20%20%20%20%20%20%20%20%20%20self.pred_mat%20%3D%20pred_mat%0A%20%20%20%20%20%20%20%20%20%20%20%20self.obs_mat%20%3D%20obs_mat%0A%0A%20%20%20%20%20%20%20%20def%20predict(self)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state%20%3D%20self.pred_mat%20%40%20self.state%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state_error%20%3D%20self.pred_mat%20%40%20self.state_error%20%40%20self.pred_mat.T%20%2B%20self.pred_error%0A%0A%20%20%20%20%20%20%20%20def%20update(self%2C%20observation%3A%20np.ndarray)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20innovation%20%3D%20observation%20-%20self.obs_mat%20%40%20self.state%0A%20%20%20%20%20%20%20%20%20%20%20%20innovation_cov%20%3D%20self.obs_mat%20%40%20self.state_error%20%40%20self.obs_mat.T%20%2B%20self.obs_error%0A%20%20%20%20%20%20%20%20%20%20%20%20kalman_gain%20%3D%20self.state_error%20%40%20self.obs_mat%20%40%20np.linalg.inv(innovation_cov)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state%20%3D%20self.state%20%2B%20kalman_gain%20%40%20innovation%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state_error%20%3D%20(np.eye(len(self.state))%20-%20kalman_gain)%20%40%20self.state_error%20%20%23%20%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%82%B2%E3%82%A4%E3%83%B3%E3%82%92%E8%A8%88%E7%AE%97%20%20%23%20%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%AA%E3%83%B3%E3%82%B0%0A%0A%20%20%20%20return%20(EKF%2C)%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%20EKF%20%E5%89%8D%E6%BA%96%E5%82%99%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(EKF%2C%20dt%2C%20noise_sigma%2C%20np%2C%20points_obs)%3A%0A%20%20%20%20%23%20%E5%88%9D%E6%9C%9F%E7%8A%B6%E6%85%8B%E4%BD%9C%E6%88%90%0A%20%20%20%20init_state%20%3D%20points_obs%5B0%2C%20%3A%5D%0A%20%20%20%20init_state_error%20%3D%20np.array(%5B%5Bnoise_sigma%20**%202%2C%200%5D%2C%20%5B0%2C%20noise_sigma%20**%202%5D%5D)%0A%20%20%20%20pred_mat%20%3D%20np.array(%5B%5B1%2C%20dt%5D%2C%20%5B-dt%2C%201%5D%5D)%0A%20%20%20%20%23%20EKF%20%E3%83%A2%E3%83%87%E3%83%AB%E8%A8%AD%E5%AE%9A%20(%E6%99%82%E8%A8%88%E5%9B%9E%E3%82%8A%E5%9B%9E%E8%BB%A2)%0A%20%20%20%20_pred_error%20%3D%20np.array(%5B%5B0.5%20*%20dt%20**%202%2C%200%5D%2C%20%5B0%2C%200.5%20*%20dt%20**%202%5D%5D)%0A%20%20%20%20obs_mat%20%3D%20np.eye(2)%0A%20%20%20%20_obs_error%20%3D%20np.array(%5B%5Bnoise_sigma%20**%202%2C%200%5D%2C%20%5B0%2C%20noise_sigma%20**%202%5D%5D)%0A%20%20%20%20%23%20EKF%20%E3%82%A4%E3%83%B3%E3%82%B9%E3%82%BF%E3%83%B3%E3%82%B9%E7%94%9F%E6%88%90%0A%20%20%20%20kf%20%3D%20EKF(state%3Dinit_state%2C%20state_error%3Dinit_state_error%2C%20pred_mat%3Dpred_mat%2C%20pred_error%3D_pred_error%2C%20obs_mat%3Dobs_mat%2C%20obs_error%3D_obs_error)%0A%20%20%20%20return%20init_state%2C%20init_state_error%2C%20kf%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%20EKF%20%E5%AE%9F%E8%A1%8C%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(init_state%2C%20init_state_error%2C%20kf%2C%20np%2C%20points_obs)%3A%0A%20%20%20%20%23%20%E5%88%9D%E6%9C%9F%E5%8C%96%0A%20%20%20%20_pred_states%20%3D%20%5Bnp.array(%5B0%2C%201%5D)%5D%0A%20%20%20%20kf_states%20%3D%20%5Binit_state%5D%0A%20%20%20%20kf_errors%20%3D%20%5Binit_state_error%5D%0A%20%20%20%20for%20_i%20in%20range(0%2C%20points_obs.shape%5B0%5D%20-%201)%3A%0A%20%20%20%20%23%20%E9%80%90%E6%AC%A1%E3%82%B9%E3%83%86%E3%83%83%E3%83%97%E8%A8%88%E7%AE%97%0A%20%20%20%20%20%20%20%20_pred_state%20%3D%20_pred_states%5B-1%5D%20%40%20kf.pred_mat%0A%20%20%20%20%20%20%20%20_pred_states.append(_pred_state)%20%20%23%20%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%81%AA%E3%81%97%E4%BA%88%E6%B8%AC%0A%20%20%20%20%20%20%20%20kf.predict()%0A%20%20%20%20%20%20%20%20kf.update(points_obs%5B_i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20kf_states.append(kf.state)%0A%20%20%20%20%20%20%20%20kf_errors.append(kf.state_error)%20%20%23%20%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%0A%20%20%20%20_pred_states%20%3D%20np.vstack(_pred_states)%0A%20%20%20%20kf_states%20%3D%20np.vstack(kf_states)%0A%20%20%20%20%23%20np.ndarray%20%E3%81%AB%E5%A4%89%E6%8F%9B%0A%20%20%20%20kf_errors%20%3D%20np.dstack(kf_errors)%20%20%23%20%E3%83%87%E3%83%BC%E3%82%BF%E4%BF%9D%E5%AD%98%0A%20%20%20%20return%20kf_errors%2C%20kf_states%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%20%E8%AA%A4%E5%B7%AE%E5%85%B1%E5%88%86%E6%95%A3%E8%A1%8C%E5%88%97%E3%82%92%E6%A5%95%E5%86%86%E3%81%A7%E8%A1%A8%E3%81%99%E3%81%9F%E3%82%81%E3%81%AE%E3%83%98%E3%83%AB%E3%83%91%E3%83%BC%E9%96%A2%E6%95%B0%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%20def%20cov2ellipse(cov%3A%20np.ndarray)%3A%0A%20%20%20%20%20%20%20%20eig_val%2C%20eig_vec%20%3D%20np.linalg.eigh(cov)%0A%20%20%20%20%20%20%20%20angle%20%3D%20np.arctan2(eig_vec%5B1%2C%200%5D%2C%20eig_vec%5B0%2C%200%5D)%0A%20%20%20%20%20%20%20%20width%20%3D%20np.sqrt(eig_val%5B0%5D)%0A%20%20%20%20%20%20%20%20height%20%3D%20np.sqrt(eig_val%5B1%5D)%0A%20%20%20%20%20%20%20%20return%20width%2C%20height%2C%20angle%0A%0A%20%20%20%20return%20(cov2ellipse%2C)%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%20%23%23%23%23%20%E5%8F%AF%E8%A6%96%E5%8C%96%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%20%E7%B5%90%E6%9E%9C%E3%82%92%E6%8F%8F%E7%94%BB%E3%80%82%E3%81%BE%E3%81%9A%E3%81%AF%E3%83%91%E3%83%AC%E3%83%83%E3%83%88%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt_ekf%0A%0A%20%20%20%20ekf_fig%2C%20ekf_ax%20%3D%20plt_ekf.subplots()%0A%20%20%20%20ekf_ax.set_aspect('equal')%0A%20%20%20%20ekf_ax.set_xlim(-1.3%2C%201.3)%0A%20%20%20%20ekf_ax.set_ylim(-1.3%2C%201.3)%0A%20%20%20%20return%20ekf_ax%2C%20ekf_fig%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%20%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E8%BB%8C%E8%B7%A1%E3%81%AE%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(blue%2C%20ekf_ax%2C%20green%2C%20orange%2C%20red)%3A%0A%20%20%20%20gt_traj_plt%2C%20%3D%20ekf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dblue%2C%20label%3D%22GT%20Trajectory%22)%0A%20%20%20%20gt_pos_plt%2C%20%3D%20ekf_ax.plot(%0A%20%20%20%20%20%20%20%20%5B%5D%2C%20%5B%5D%2C%20c%3Dorange%2C%20marker%3D%22o%22%2C%20markersize%3D5%2C%20label%3D%22True%20Point%22%0A%20%20%20%20)%0A%0A%20%20%20%20obs_pos_scat%20%3D%20ekf_ax.scatter(%5B%5D%2C%20%5B%5D%2C%20s%3D5%2C%20c%3Dred%2C%20label%3D%22Observation%22)%0A%0A%20%20%20%20kf_pos_plt%2C%20%3D%20ekf_ax.plot(%0A%20%20%20%20%20%20%20%20%5B%5D%2C%20%5B%5D%2C%20c%3Dgreen%2C%20marker%3D%22o%22%2C%20markersize%3D5%2C%20label%3D%22Kalman%20Filter%22%0A%20%20%20%20)%0A%20%20%20%20return%20gt_pos_plt%2C%20gt_traj_plt%2C%20kf_pos_plt%2C%20obs_pos_scat%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%20%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%E3%81%AE%E6%8F%8F%E7%94%BB%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(ekf_ax%2C%20green%2C%20noise_sigma%2C%20red)%3A%0A%20%20%20%20from%20matplotlib.patches%20import%20Ellipse%0A%0A%20%20%20%20%23%20%E8%A6%B3%E6%B8%AC%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20obs_ellipse%20%3D%20Ellipse(%0A%20%20%20%20%20%20%20%20xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%0A%20%20%20%20%20%20%20%20color%3Dred%2C%20alpha%3D0.1%2C%20animated%3DTrue)%0A%20%20%20%20ekf_ax.add_patch(obs_ellipse)%0A%0A%20%20%20%20%23%20%E6%8E%A8%E5%AE%9A%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20kalman_ellipse%20%3D%20Ellipse(%0A%20%20%20%20%20%20%20%20xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%0A%20%20%20%20%20%20%20%20color%3Dgreen%2C%20alpha%3D0.3%2C%20animated%3DTrue)%0A%20%20%20%20ekf_ax.add_patch(kalman_ellipse)%0A%20%20%20%20return%20Ellipse%2C%20kalman_ellipse%2C%20obs_ellipse%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%20GT%E3%83%BB%E4%BA%88%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E8%A6%B3%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E3%83%95%E3%83%A5%E3%83%BC%E3%82%B8%E3%83%A7%E3%83%B3%E7%B5%90%E6%9E%9C%E3%82%92%E8%AA%A4%E5%B7%AE%E5%88%86%E5%B8%83%E4%BB%98%E3%81%8D%E3%81%A7%E6%8F%8F%E7%94%BB%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20artifacts_dir%2C%0A%20%20%20%20cov2ellipse%2C%0A%20%20%20%20ekf_ax%2C%0A%20%20%20%20ekf_fig%2C%0A%20%20%20%20gif_image%2C%0A%20%20%20%20gt_pos_plt%2C%0A%20%20%20%20gt_traj_plt%2C%0A%20%20%20%20kalman_ellipse%2C%0A%20%20%20%20kf_errors%2C%0A%20%20%20%20kf_pos_plt%2C%0A%20%20%20%20kf_states%2C%0A%20%20%20%20obs_ellipse%2C%0A%20%20%20%20obs_pos_scat%2C%0A%20%20%20%20points%2C%0A%20%20%20%20points_obs%2C%0A%20%20%20%20save_gif%2C%0A)%3A%0A%20%20%20%20ekf_ax.legend(loc%3D'upper%20right'%2C%20fontsize%3D'x-small')%0A%0A%20%20%20%20def%20ekf_anim_callback(i)%3A%0A%20%20%20%20%20%20%20%20ekf_ax.set_title(f'Frame%20%7Bi%7D')%0A%20%20%20%20%20%20%20%20gt_traj_plt.set_data(points%5B%3A%20i%20%2B%201%2C%200%5D%2C%20points%5B%3A%20i%20%2B%201%2C%201%5D)%0A%20%20%20%20%20%20%20%20gt_pos_plt.set_data(%5Bpoints%5Bi%2C%200%5D%5D%2C%20%5Bpoints%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20obs_pos_scat.set_offsets(points_obs%5B%3A%20i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20obs_ellipse.set_center(points_obs%5Bi%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20kf_pos_plt.set_data(%5Bkf_states%5Bi%2C%200%5D%5D%2C%20%5Bkf_states%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20width%2C%20height%2C%20angle%20%3D%20cov2ellipse(kf_errors%5B%3A%2C%20%3A%2C%20i%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse.set_center(%5Bkf_states%5Bi%2C%200%5D%2C%20kf_states%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse.set_width(width)%0A%20%20%20%20%20%20%20%20kalman_ellipse.set_height(height)%0A%20%20%20%20%20%20%20%20kalman_ellipse.set_angle(angle)%0A%0A%20%20%20%20ekf_gif_path%20%3D%20artifacts_dir%20%2F%20%22ekf.gif%22%0A%20%20%20%20print(save_gif(ekf_fig%2C%20ekf_anim_callback%2C%20len(points_obs)%2C%20ekf_gif_path%2C%20100))%0A%20%20%20%20ekf_animation%20%3D%20gif_image(ekf_gif_path%2C%20%22EKF%20%E3%81%AB%E3%82%88%E3%82%8B%E5%86%86%E9%81%8B%E5%8B%95%E3%81%AE%E6%8E%A8%E5%AE%9A%22)%0A%20%20%20%20return%20(ekf_animation%2C)%0A%0A%0A%40app.cell%0Adef%20_(ekf_animation)%3A%0A%20%20%20%20ekf_animation%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%20%23%23%23%20UKF%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%20https%3A%2F%2Finzkyk.xyz%2Fkalman_filter%2Funscented_kalman_filter%2F%23section%3A10.5%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%20%E3%82%B7%E3%82%B0%E3%83%9E%E7%82%B9%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%26%5Cvec%7Bs%7D%5E0%3D%5Cvec%7B%5Cmu%7D%2C%5Cquad%0A%20%20%20%20w_m%5E0%3D%5Cfrac%7B%5Clambda%7D%7Bn%2B%5Clambda%7D%2C%5Cquad%0A%20%20%20%20w_c%5E0%3D%5Cfrac%7B%5Clambda%7D%7Bn%2B%5Clambda%7D%2B(1-%5Calpha%5E2%2B%5Cbeta)%2C%20%5C%5C%0A%20%20%20%20%26%5Cvec%7Bs%7D%5Ei%3D%5Cvec%7B%5Cmu%7D%2B%5Csqrt%7Bn%2B%5Clambda%7D%5Csqrt%7B%5CSigma%7D_%7B%5Clceil%20i%5Crceil%7D%2C%20%5Cquad%0A%20%20%20%20%5Cvec%7Bs%7D%5E%7Bi%2Bn%7D%3D%5Cvec%7B%5Cmu%7D-%5Csqrt%7Bn%2B%5Clambda%7D%5Csqrt%7B%5CSigma%7D_%7B%5Clceil%20i%5Crceil%7D%2C%20%5C%5C%0A%20%20%20%20%26w_m%5Ei%3Dw_c%5Ei%3D%5Cfrac%7B1%7D%7B2(n%2B%5Clambda)%7D%2C%5Cquad%0A%20%20%20%20i%3D1%2C2%2C%5Cldots%2Cn%5C%2C%5C%5C%0A%20%20%20%20%26%5Ckappa%5Cleq0%2C%5Cquad%5Calpha%5Cin%5B0%2C1%5D%2C%5Cquad%0A%20%20%20%20%5Clambda%3D%5Calpha%5E2(n%2B%5Ckappa)-n%2C%5Cquad%5Cbeta%3D2%0A%20%20%20%20%5Cend%7Baligned%7D%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_(np)%3A%0A%20%20%20%20from%20typing%20import%20Callable%0A%0A%20%20%20%20class%20UKF%3A%0A%0A%20%20%20%20%20%20%20%20def%20__init__(self%2C%20state%3A%20np.ndarray%2C%20state_error%3A%20np.ndarray%2C%20pred_map%3A%20Callable%5B%5Bnp.ndarray%5D%2C%20np.ndarray%5D%2C%20pred_error%3A%20np.ndarray%2C%20obs_map%3A%20Callable%5B%5Bnp.ndarray%5D%2C%20np.ndarray%5D%2C%20obs_error%3A%20np.ndarray%2C%20alpha%3A%20float%3D0.1%2C%20beta%3A%20float%3D2%2C%20kappa%3A%20float%3D-1)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state%20%3D%20state%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state_error%20%3D%20state_error%0A%20%20%20%20%20%20%20%20%20%20%20%20self.pred_error%20%3D%20pred_error%0A%20%20%20%20%20%20%20%20%20%20%20%20self.obs_error%20%3D%20obs_error%0A%20%20%20%20%20%20%20%20%20%20%20%20self.pred_map%20%3D%20pred_map%0A%20%20%20%20%20%20%20%20%20%20%20%20self.obs_map%20%3D%20obs_map%0A%20%20%20%20%20%20%20%20%20%20%20%20n%20%3D%20state.shape%5B0%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20self.lambda_%20%3D%20alpha%20**%202%20*%20(n%20%2B%20kappa)%20-%20n%0A%20%20%20%20%20%20%20%20%20%20%20%20self.w_m%20%3D%20np.full(2%20*%20n%20%2B%201%2C%201%20%2F%20(2%20*%20(n%20%2B%20self.lambda_)))%0A%20%20%20%20%20%20%20%20%20%20%20%20self.w_c%20%3D%20np.full(2%20*%20n%20%2B%201%2C%201%20%2F%20(2%20*%20(n%20%2B%20self.lambda_)))%0A%20%20%20%20%20%20%20%20%20%20%20%20self.w_m%5B0%5D%20%3D%20self.lambda_%20%2F%20(n%20%2B%20self.lambda_)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.w_c%5B0%5D%20%3D%20self.lambda_%20%2F%20(n%20%2B%20self.lambda_)%20%2B%20(1%20-%20alpha%20**%202%20%2B%20beta)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.sigmas%20%3D%20self.get_sigmas()%0A%0A%20%20%20%20%20%20%20%20def%20predict(self)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20self.sigmas%20%3D%20self.get_sigmas()%0A%20%20%20%20%20%20%20%20%20%20%20%20self.sigmas%20%3D%20np.array(%5Bself.pred_map(sigma)%20for%20sigma%20in%20self.sigmas%5D)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state%2C%20self.state_error%20%3D%20self.u_transform(self.sigmas%2C%20self.pred_error)%0A%0A%20%20%20%20%20%20%20%20def%20update(self%2C%20observation%3A%20np.ndarray)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20obs_sigmas%20%3D%20np.array(%5Bself.obs_map(sigma)%20for%20sigma%20in%20self.sigmas%5D)%0A%20%20%20%20%20%20%20%20%20%20%20%20z_mu%2C%20z_error%20%3D%20self.u_transform(obs_sigmas%2C%20self.obs_error)%0A%20%20%20%20%20%20%20%20%20%20%20%20xz_error%20%3D%20np.zeros((len(self.state)%2C%20len(z_mu)))%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20idx%20in%20range(len(self.w_c))%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20xz_error%20%3D%20xz_error%20%2B%20self.w_c%5Bidx%5D%20*%20np.outer(self.sigmas%5Bidx%2C%20%3A%5D%20-%20self.state%2C%20obs_sigmas%5Bidx%2C%20%3A%5D%20-%20z_mu)%0A%20%20%20%20%20%20%20%20%20%20%20%20kalman_gain%20%3D%20xz_error%20%40%20np.linalg.inv(z_error)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state%20%3D%20self.state%20%2B%20kalman_gain%20%40%20(observation%20-%20z_mu)%0A%20%20%20%20%20%20%20%20%20%20%20%20self.state_error%20%3D%20self.state_error%20-%20kalman_gain%20%40%20z_error%20%40%20kalman_gain.T%0A%0A%20%20%20%20%20%20%20%20def%20u_transform(self%2C%20sigmas%2C%20noise_cov)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20mu%20%3D%200%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20idx%20in%20range(len(self.w_m))%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20mu%20%3D%20mu%20%2B%20self.w_m%5Bidx%5D%20*%20sigmas%5Bidx%2C%20%3A%5D%0A%20%20%20%20%20%20%20%20%20%20%20%20cov_mat%20%3D%20np.zeros_like(noise_cov)%0A%20%20%20%20%20%20%20%20%20%20%20%20for%20idx%20in%20range(len(self.w_c))%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20cov_mat%20%3D%20cov_mat%20%2B%20self.w_c%5Bidx%5D%20*%20np.outer(sigmas%5Bidx%2C%20%3A%5D%20-%20mu%2C%20sigmas%5Bidx%2C%20%3A%5D%20-%20mu)%0A%20%20%20%20%20%20%20%20%20%20%20%20cov_mat%20%3D%20cov_mat%20%2B%20noise_cov%0A%20%20%20%20%20%20%20%20%20%20%20%20return%20(mu%2C%20cov_mat)%0A%0A%20%20%20%20%20%20%20%20def%20get_sigmas(self)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20n%20%3D%20len(self.state)%0A%20%20%20%20%20%20%20%20%20%20%20%20sqrt_mat%20%3D%20np.linalg.cholesky((n%20%2B%20self.lambda_)%20*%20self.state_error)%0A%20%20%20%20%20%20%20%20%20%20%20%20sigma_plus%20%3D%20self.state%20%2B%20sqrt_mat%0A%20%20%20%20%20%20%20%20%20%20%20%20sigma_minus%20%3D%20self.state%20-%20sqrt_mat%0A%20%20%20%20%20%20%20%20%20%20%20%20sigmas%20%3D%20np.vstack((self.state%2C%20sigma_plus%2C%20sigma_minus))%0A%20%20%20%20%20%20%20%20%20%20%20%20return%20sigmas%0A%0A%20%20%20%20return%20(UKF%2C)%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%20UKF%20%E3%81%AE%E8%A8%AD%E5%AE%9A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(UKF%2C%20dt%2C%20noise_sigma%2C%20np%2C%20points_obs)%3A%0A%20%20%20%20init_state_1%20%3D%20points_obs%5B0%2C%20%3A%5D%0A%20%20%20%20init_state_error_1%20%3D%20np.array(%5B%5Bnoise_sigma%20**%202%2C%200%5D%2C%20%5B0%2C%20noise_sigma%20**%202%5D%5D)%0A%0A%20%20%20%20def%20pred_map(x)%3A%0A%20%20%20%20%20%20%20%20return%20np.array(%5B%5Bnp.cos(dt)%2C%20np.sin(dt)%5D%2C%20%5B-np.sin(dt)%2C%20np.cos(dt)%5D%5D)%20%40%20x%0A%0A%20%20%20%20def%20obs_map(x)%3A%0A%20%20%20%20%20%20%20%20return%20x%0A%0A%20%20%20%20pred_error%20%3D%20np.array(%5B%5B0.5%20*%20dt%20**%202%2C%200%5D%2C%20%5B0%2C%200.5%20*%20dt%20**%202%5D%5D)%0A%20%20%20%20obs_error%20%3D%20np.array(%5B%5Bnoise_sigma%20**%202%2C%200%5D%2C%20%5B0%2C%20noise_sigma%20**%202%5D%5D)%0A%20%20%20%20kf_1%20%3D%20UKF(%0A%20%20%20%20%20%20%20%20state%3Dinit_state_1%2C%0A%20%20%20%20%20%20%20%20state_error%3Dinit_state_error_1%2C%0A%20%20%20%20%20%20%20%20pred_map%3Dpred_map%2C%0A%20%20%20%20%20%20%20%20pred_error%3Dpred_error%2C%0A%20%20%20%20%20%20%20%20obs_map%3Dobs_map%2C%0A%20%20%20%20%20%20%20%20obs_error%3Dobs_error%2C%0A%20%20%20%20)%0A%20%20%20%20return%20init_state_1%2C%20init_state_error_1%2C%20kf_1%2C%20pred_map%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%20UKF%20%E5%AE%9F%E8%A1%8C%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(init_state_1%2C%20init_state_error_1%2C%20kf_1%2C%20np%2C%20points_obs%2C%20pred_map)%3A%0A%20%20%20%20_pred_states%20%3D%20%5Bnp.array(%5B0%2C%201%5D)%5D%0A%20%20%20%20pred_sigmas%20%3D%20%5Bnp.array(%5B%5B0%2C%201%5D%5D)%5D%0A%20%20%20%20kf_states_1%20%3D%20%5Binit_state_1%5D%0A%20%20%20%20kf_errors_1%20%3D%20%5Binit_state_error_1%5D%0A%20%20%20%20for%20_i%20in%20range(0%2C%20points_obs.shape%5B0%5D%20-%201)%3A%0A%20%20%20%20%20%20%20%20_pred_state%20%3D%20pred_map(_pred_states%5B-1%5D)%0A%20%20%20%20%20%20%20%20_pred_states.append(_pred_state)%0A%20%20%20%20%20%20%20%20kf_1.predict()%0A%20%20%20%20%20%20%20%20kf_1.update(points_obs%5B_i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20pred_sigmas.append(np.array(kf_1.sigmas))%0A%20%20%20%20%20%20%20%20kf_states_1.append(kf_1.state)%0A%20%20%20%20%20%20%20%20kf_errors_1.append(kf_1.state_error)%0A%20%20%20%20_pred_states%20%3D%20np.vstack(_pred_states)%0A%20%20%20%20kf_states_1%20%3D%20np.vstack(kf_states_1)%0A%20%20%20%20kf_errors_1%20%3D%20np.dstack(kf_errors_1)%0A%20%20%20%20return%20kf_errors_1%2C%20kf_states_1%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%20%23%23%23%23%20%E5%8F%AF%E8%A6%96%E5%8C%96%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%20%E7%B5%90%E6%9E%9C%E3%82%92%E6%8F%8F%E7%94%BB%E3%80%82%E3%81%BE%E3%81%9A%E3%81%AF%E3%83%91%E3%83%AC%E3%83%83%E3%83%88%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt_ukf%0A%0A%20%20%20%20ukf_fig%2C%20ukf_ax%20%3D%20plt_ukf.subplots()%0A%20%20%20%20ukf_ax.set_aspect('equal')%0A%20%20%20%20ukf_ax.set_xlim(-1.3%2C%201.3)%0A%20%20%20%20ukf_ax.set_ylim(-1.3%2C%201.3)%0A%20%20%20%20return%20ukf_ax%2C%20ukf_fig%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%20%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E8%BB%8C%E8%B7%A1%E3%81%AE%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(blue%2C%20green%2C%20orange%2C%20red%2C%20ukf_ax)%3A%0A%20%20%20%20gt_traj_plt_1%2C%20%3D%20ukf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dblue%2C%20label%3D'GT%20Trajectory')%0A%20%20%20%20gt_pos_plt_1%2C%20%3D%20ukf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dorange%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'True%20Point')%0A%20%20%20%20obs_pos_scat_1%20%3D%20ukf_ax.scatter(%5B%5D%2C%20%5B%5D%2C%20s%3D5%2C%20c%3Dred%2C%20label%3D'Observation')%0A%20%20%20%20kf_pos_plt_1%2C%20%3D%20ukf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dgreen%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'Kalman%20Filter')%0A%20%20%20%20return%20gt_pos_plt_1%2C%20gt_traj_plt_1%2C%20kf_pos_plt_1%2C%20obs_pos_scat_1%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%20%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%E3%81%AE%E6%8F%8F%E7%94%BB%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Ellipse%2C%20green%2C%20noise_sigma%2C%20red%2C%20ukf_ax)%3A%0A%20%20%20%20obs_ellipse_1%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dred%2C%20alpha%3D0.1%2C%20animated%3DTrue)%0A%20%20%20%20ukf_ax.add_patch(obs_ellipse_1)%0A%20%20%20%20%23%20%E8%A6%B3%E6%B8%AC%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20kalman_ellipse_1%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dgreen%2C%20alpha%3D0.3%2C%20animated%3DTrue)%0A%20%20%20%20%23%20%E6%8E%A8%E5%AE%9A%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20ukf_ax.add_patch(kalman_ellipse_1)%0A%20%20%20%20return%20kalman_ellipse_1%2C%20obs_ellipse_1%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%20GT%E3%83%BB%E4%BA%88%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E8%A6%B3%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E3%83%95%E3%83%A5%E3%83%BC%E3%82%B8%E3%83%A7%E3%83%B3%E7%B5%90%E6%9E%9C%E3%82%92%E8%AA%A4%E5%B7%AE%E5%88%86%E5%B8%83%E4%BB%98%E3%81%8D%E3%81%A7%E6%8F%8F%E7%94%BB%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20artifacts_dir%2C%0A%20%20%20%20cov2ellipse%2C%0A%20%20%20%20gif_image%2C%0A%20%20%20%20gt_pos_plt_1%2C%0A%20%20%20%20gt_traj_plt_1%2C%0A%20%20%20%20kalman_ellipse_1%2C%0A%20%20%20%20kf_errors_1%2C%0A%20%20%20%20kf_pos_plt_1%2C%0A%20%20%20%20kf_states_1%2C%0A%20%20%20%20obs_ellipse_1%2C%0A%20%20%20%20obs_pos_scat_1%2C%0A%20%20%20%20points%2C%0A%20%20%20%20points_obs%2C%0A%20%20%20%20save_gif%2C%0A%20%20%20%20ukf_ax%2C%0A%20%20%20%20ukf_fig%2C%0A)%3A%0A%20%20%20%20ukf_ax.legend(loc%3D'upper%20right'%2C%20fontsize%3D'x-small')%0A%0A%20%20%20%20def%20ukf_anim_callback(i)%3A%0A%20%20%20%20%20%20%20%20ukf_ax.set_title(f'Frame%20%7Bi%7D')%0A%20%20%20%20%20%20%20%20gt_traj_plt_1.set_data(points%5B%3A%20i%20%2B%201%2C%200%5D%2C%20points%5B%3A%20i%20%2B%201%2C%201%5D)%0A%20%20%20%20%20%20%20%20gt_pos_plt_1.set_data(%5Bpoints%5Bi%2C%200%5D%5D%2C%20%5Bpoints%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20obs_pos_scat_1.set_offsets(points_obs%5B%3A%20i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20obs_ellipse_1.set_center(points_obs%5Bi%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20kf_pos_plt_1.set_data(%5Bkf_states_1%5Bi%2C%200%5D%5D%2C%20%5Bkf_states_1%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20width%2C%20height%2C%20angle%20%3D%20cov2ellipse(kf_errors_1%5B%3A%2C%20%3A%2C%20i%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse_1.set_center(%5Bkf_states_1%5Bi%2C%200%5D%2C%20kf_states_1%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse_1.set_width(width)%0A%20%20%20%20%20%20%20%20kalman_ellipse_1.set_height(height)%0A%20%20%20%20%20%20%20%20kalman_ellipse_1.set_angle(angle)%0A%0A%20%20%20%20ukf_gif_path%20%3D%20artifacts_dir%20%2F%20%22ukf.gif%22%0A%20%20%20%20print(save_gif(ukf_fig%2C%20ukf_anim_callback%2C%20len(points_obs)%2C%20ukf_gif_path%2C%20100))%0A%20%20%20%20ukf_animation%20%3D%20gif_image(ukf_gif_path%2C%20%22UKF%20%E3%81%AB%E3%82%88%E3%82%8B%E5%86%86%E9%81%8B%E5%8B%95%E3%81%AE%E6%8E%A8%E5%AE%9A%22)%0A%20%20%20%20return%20(ukf_animation%2C)%0A%0A%0A%40app.cell%0Adef%20_(ukf_animation)%3A%0A%20%20%20%20ukf_animation%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%20%23%23%20Kalman%20Filter%20by%20FilterPy%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%20%5BThe%20Documentation%5D(https%3A%2F%2Ffilterpy.readthedocs.io%2Fen%2Flatest%2F)%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%20%23%23%23%20UKF%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%20filterpy%20%E3%81%A7%20UKF%20%E3%81%AE%E3%82%A4%E3%83%B3%E3%82%B9%E3%82%BF%E3%83%B3%E3%82%B9%E3%82%92%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20API%20%E3%81%AF%5B%E3%81%93%E3%81%93%5D(https%3A%2F%2Ffilterpy.readthedocs.io%2Fen%2Flatest%2Fkalman%2FUnscentedKalmanFilter.html)%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(dt%2C%20noise_sigma%2C%20np%2C%20points_obs)%3A%0A%20%20%20%20from%20filterpy%20import%20kalman%0A%0A%20%20%20%20def%20pred_map_1(x%2C%20dt)%3A%0A%20%20%20%20%20%20%20%20return%20x%20%40%20np.array(%5B%5Bnp.cos(dt)%2C%20-np.sin(dt)%5D%2C%20%5Bnp.sin(dt)%2C%20np.cos(dt)%5D%5D)%0A%0A%20%20%20%20def%20obs_map_1(x)%3A%0A%20%20%20%20%20%20%20%20return%20x%0A%0A%20%20%20%20sigma_points%20%3D%20kalman.MerweScaledSigmaPoints(2%2C%20alpha%3D0.1%2C%20beta%3D2.0%2C%20kappa%3D-1)%0A%20%20%20%20kf_2%20%3D%20kalman.UnscentedKalmanFilter(%0A%20%20%20%20%20%20%20%20dim_x%3D2%2C%20dim_z%3D2%2C%20dt%3Ddt%2C%20hx%3Dobs_map_1%2C%20fx%3Dpred_map_1%2C%20points%3Dsigma_points%0A%20%20%20%20)%0A%20%20%20%20kf_2.x%20%3D%20points_obs%5B0%2C%20%3A%5D%0A%20%20%20%20kf_2.P%20%3D%20kf_2.P%20*%20noise_sigma%20**%202%0A%20%20%20%20kf_2.R%20%3D%20np.array(%5B%5Bnoise_sigma%20**%202%2C%200%5D%2C%20%5B0%2C%20noise_sigma%20**%202%5D%5D)%0A%20%20%20%20kf_2.Q%20%3D%20np.array(%5B%5B0.5%20*%20dt%20**%202%2C%200%5D%2C%20%5B0%2C%200.5%20*%20dt%20**%202%5D%5D)%0A%20%20%20%20return%20kalman%2C%20kf_2%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%20UKF%20%E5%AE%9F%E8%A1%8C%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(kf_2%2C%20np%2C%20points_obs)%3A%0A%20%20%20%20kf_states_2%20%3D%20%5Bkf_2.x%5D%0A%20%20%20%20kf_errors_2%20%3D%20%5Bkf_2.P%5D%0A%20%20%20%20for%20_i%20in%20range(0%2C%20points_obs.shape%5B0%5D%20-%201)%3A%0A%20%20%20%20%20%20%20%20kf_2.predict()%0A%20%20%20%20%20%20%20%20kf_2.update(points_obs%5B_i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20kf_states_2.append(kf_2.x)%0A%20%20%20%20%20%20%20%20kf_errors_2.append(kf_2.P)%0A%20%20%20%20kf_states_2%20%3D%20np.vstack(kf_states_2)%0A%20%20%20%20kf_errors_2%20%3D%20np.dstack(kf_errors_2)%0A%20%20%20%20return%20kf_errors_2%2C%20kf_states_2%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%20%23%23%23%23%20%E5%8F%AF%E8%A6%96%E5%8C%96%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%20%E7%B5%90%E6%9E%9C%E3%82%92%E6%8F%8F%E7%94%BB%E3%80%82%E3%81%BE%E3%81%9A%E3%81%AF%E3%83%91%E3%83%AC%E3%83%83%E3%83%88%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt_filterpy%0A%0A%20%20%20%20filterpy_fig%2C%20filterpy_ax%20%3D%20plt_filterpy.subplots()%0A%20%20%20%20filterpy_ax.set_aspect('equal')%0A%20%20%20%20filterpy_ax.set_xlim(-1.3%2C%201.3)%0A%20%20%20%20filterpy_ax.set_ylim(-1.3%2C%201.3)%0A%20%20%20%20return%20filterpy_ax%2C%20filterpy_fig%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%20%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E8%BB%8C%E8%B7%A1%E3%81%AE%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(blue%2C%20filterpy_ax%2C%20green%2C%20orange%2C%20red)%3A%0A%20%20%20%20gt_traj_plt_2%2C%20%3D%20filterpy_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dblue%2C%20label%3D'GT%20Trajectory')%0A%20%20%20%20gt_pos_plt_2%2C%20%3D%20filterpy_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dorange%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'True%20Point')%0A%20%20%20%20filterpy_obs_pos_scat%20%3D%20filterpy_ax.scatter(%5B%5D%2C%20%5B%5D%2C%20s%3D5%2C%20c%3Dred%2C%20label%3D'Observation')%0A%20%20%20%20kf_pos_plt_2%2C%20%3D%20filterpy_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dgreen%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'Kalman%20Filter')%0A%20%20%20%20return%20filterpy_obs_pos_scat%2C%20gt_pos_plt_2%2C%20gt_traj_plt_2%2C%20kf_pos_plt_2%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%20%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%E3%81%AE%E6%8F%8F%E7%94%BB%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Ellipse%2C%20filterpy_ax%2C%20green%2C%20noise_sigma%2C%20red)%3A%0A%20%20%20%20obs_ellipse_2%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dred%2C%20alpha%3D0.1%2C%20animated%3DTrue)%0A%20%20%20%20filterpy_ax.add_patch(obs_ellipse_2)%0A%20%20%20%20%23%20%E8%A6%B3%E6%B8%AC%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20kalman_ellipse_2%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dgreen%2C%20alpha%3D0.3%2C%20animated%3DTrue)%0A%20%20%20%20%23%20%E6%8E%A8%E5%AE%9A%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20filterpy_ax.add_patch(kalman_ellipse_2)%0A%20%20%20%20return%20kalman_ellipse_2%2C%20obs_ellipse_2%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%20GT%E3%83%BB%E4%BA%88%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E8%A6%B3%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%83%BB%E3%83%95%E3%83%A5%E3%83%BC%E3%82%B8%E3%83%A7%E3%83%B3%E7%B5%90%E6%9E%9C%E3%82%92%E8%AA%A4%E5%B7%AE%E5%88%86%E5%B8%83%E4%BB%98%E3%81%8D%E3%81%A7%E6%8F%8F%E7%94%BB%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20artifacts_dir%2C%0A%20%20%20%20cov2ellipse%2C%0A%20%20%20%20filterpy_ax%2C%0A%20%20%20%20filterpy_fig%2C%0A%20%20%20%20filterpy_obs_pos_scat%2C%0A%20%20%20%20gif_image%2C%0A%20%20%20%20gt_pos_plt_2%2C%0A%20%20%20%20gt_traj_plt_2%2C%0A%20%20%20%20kalman_ellipse_2%2C%0A%20%20%20%20kf_errors_2%2C%0A%20%20%20%20kf_pos_plt_2%2C%0A%20%20%20%20kf_states_2%2C%0A%20%20%20%20obs_ellipse_2%2C%0A%20%20%20%20points%2C%0A%20%20%20%20points_obs%2C%0A%20%20%20%20save_gif%2C%0A)%3A%0A%20%20%20%20filterpy_ax.legend(loc%3D'upper%20right'%2C%20fontsize%3D'x-small')%0A%0A%20%20%20%20def%20filterpy_anim_callback(i)%3A%0A%20%20%20%20%20%20%20%20filterpy_ax.set_title(f'Frame%20%7Bi%7D')%0A%20%20%20%20%20%20%20%20gt_traj_plt_2.set_data(points%5B%3A%20i%20%2B%201%2C%200%5D%2C%20points%5B%3A%20i%20%2B%201%2C%201%5D)%0A%20%20%20%20%20%20%20%20gt_pos_plt_2.set_data(%5Bpoints%5Bi%2C%200%5D%5D%2C%20%5Bpoints%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20filterpy_obs_pos_scat.set_offsets(points_obs%5B%3A%20i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20obs_ellipse_2.set_center(points_obs%5Bi%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20kf_pos_plt_2.set_data(%5Bkf_states_2%5Bi%2C%200%5D%5D%2C%20%5Bkf_states_2%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20width%2C%20height%2C%20angle%20%3D%20cov2ellipse(kf_errors_2%5B%3A%2C%20%3A%2C%20i%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse_2.set_center(%5Bkf_states_2%5Bi%2C%200%5D%2C%20kf_states_2%5Bi%2C%201%5D%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse_2.set_width(width)%0A%20%20%20%20%20%20%20%20kalman_ellipse_2.set_height(height)%0A%20%20%20%20%20%20%20%20kalman_ellipse_2.set_angle(angle)%0A%0A%20%20%20%20filterpy_gif_path%20%3D%20artifacts_dir%20%2F%20%22filterpy_ekf.gif%22%0A%20%20%20%20print(save_gif(filterpy_fig%2C%20filterpy_anim_callback%2C%20len(points_obs)%2C%20filterpy_gif_path%2C%20100))%0A%20%20%20%20filterpy_animation%20%3D%20gif_image(filterpy_gif_path%2C%20%22FilterPy%20%E3%81%AE%20EKF%20%E3%81%AB%E3%82%88%E3%82%8B%E5%86%86%E9%81%8B%E5%8B%95%E3%81%AE%E6%8E%A8%E5%AE%9A%22)%0A%20%20%20%20return%20(filterpy_animation%2C)%0A%0A%0A%40app.cell%0Adef%20_(filterpy_animation)%3A%0A%20%20%20%20filterpy_animation%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%20%23%23%20%E4%BA%8C%E9%87%8D%E6%8C%AF%E3%82%8A%E5%AD%90%20by%20FilterPy%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%20%23%23%23%20%E3%83%87%E3%83%BC%E3%82%BF%E4%BD%9C%E6%88%90%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%20%5B%E4%BA%8C%E9%87%8D%E6%8C%AF%E3%82%8A%E5%AD%90%E3%81%AE%E3%83%A9%E3%82%B0%E3%83%A9%E3%83%B3%E3%82%B8%E3%82%A2%E3%83%B3%5D(https%3A%2F%2Fmonologue-physics.hatenablog.com%2Fentry%2F2021%2F09%2F22%2F155932)%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20L%26%3D%5Cfrac%7Bm_1%7D%7B2%7D(%5Cdot%7Bx_1%7D%5E2%2B%5Cdot%7By_1%7D%5E2)%2B%5Cfrac%7Bm_2%7D%7B2%7D(%5Cdot%7Bx_2%7D%5E2%2B%5Cdot%7By_2%7D%5E2)-m_1gy_1-m_2gy_2%20%5C%5C%0A%20%20%20%20%26%3D%0A%20%20%20%20%5Cfrac%7B1%7D%7B2%7D(m_1%2Bm_2)l_1%5E2%5Cdot%7B%5Ctheta_1%7D%5E2%0A%20%20%20%20%2B%5Cfrac%7B1%7D%7B2%7Dm_2l_2%5E2%5Cdot%7B%5Ctheta_2%7D%5E2%0A%20%20%20%20%2Bm_2l_1l_2%5Cdot%7B%5Ctheta_1%7D%5Cdot%7B%5Ctheta_2%7D%5Ccos(%5Ctheta_1-%5Ctheta_2)%0A%20%20%20%20%2B(m_1%2Bm_2)gl_1%5Ccos(%5Ctheta_1)%2Bm_2gl_2%5Ccos(%5Ctheta_2)%0A%20%20%20%20%5Cend%7Baligned%7D%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%24%5Ctheta_1%24%20%E3%81%AB%E3%81%A4%E3%81%84%E3%81%A6%E3%81%AE%E3%83%A9%E3%82%B0%E3%83%A9%E3%83%B3%E3%82%B8%E3%83%A5%E6%96%B9%E7%A8%8B%E5%BC%8F%0A%20%20%20%20%24%24%0A%20%20%20%20(m_1%2Bm_2)l_1%5E2%5Cddot%7B%5Ctheta_1%7D%2B(m_1%2Bm_2)gl_1%5Csin(%5Ctheta_1)%2Bm_2l_1l_2%5Cleft(%5Cddot%7B%5Ctheta_2%7D%5Ccos(%5Ctheta_1-%5Ctheta_2)%2B%5Cdot%7B%5Ctheta_2%7D%5E2%5Csin(%5Ctheta_1-%5Ctheta_2)%5Cright)%3D0%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%24%5Ctheta_2%24%20%E3%81%AB%E3%81%A4%E3%81%84%E3%81%A6%E3%81%AE%E3%83%A9%E3%82%B0%E3%83%A9%E3%83%B3%E3%82%B8%E3%83%A5%E6%96%B9%E7%A8%8B%E5%BC%8F%0A%20%20%20%20%24%24%0A%20%20%20%20m_2l_2%5E2%5Cddot%7B%5Ctheta_2%7D%2Bm_2l_1l_2%5Cleft(%5Cddot%7B%5Ctheta_1%7D%5Ccos(%5Ctheta_1-%5Ctheta_2)-%5Cdot%7B%5Ctheta_1%7D%5E2%5Csin(%5Ctheta_1-%5Ctheta_2)%5Cright)%2Bm_2gl_2%5Csin(%5Ctheta_2)%3D0%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%E6%95%B0%E5%80%A4%E8%A8%88%E7%AE%97%E3%81%99%E3%82%8B%E3%81%AB%E3%81%AF%E9%80%A3%E7%AB%8B%E4%B8%80%E6%AC%A1%E6%96%B9%E7%A8%8B%E5%BC%8F%E3%81%A7%E3%81%82%E3%82%8B%5B%E6%AD%A3%E6%BA%96%E6%96%B9%E7%A8%8B%E5%BC%8F%5D(https%3A%2F%2Fwww.aihara.co.jp%2F~taiji%2Fpendula-equations%2Fpresent-node2.html)%E3%81%AE%E6%96%B9%E3%81%8C%E6%9C%9B%E3%81%BE%E3%81%97%E3%81%84%E3%80%82%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%20%24%5Cvec%7B%5Ctheta%7D%5Cequiv(%5Ctheta_1%2C%20%5Ctheta_2)%5ET%24%20%E3%81%AB%E5%AF%BE%E3%81%99%E3%82%8B%E6%AD%A3%E6%BA%96%E6%96%B9%E7%A8%8B%E5%BC%8F%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cdot%7B%5Cvec%7B%5Ctheta%7D%7D%0A%20%20%20%20%3D%5Cfrac%7B1%7D%7B(m_1%2Bm_2)l_1%5E2m_2l_2%5E2-m_2%5E2l_1%5E2l_2%5E2%5Ccos(%5Ctheta_1-%5Ctheta_2)%7D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20m_2l_2%5E2p_1-m_2l_1l_2%5Ccos(%5Ctheta_1-%5Ctheta_2)p_2%20%5C%5C%0A%20%20%20%20-m_2l_1l_2%5Ccos(%5Ctheta_1-%5Ctheta_2)%2B(m_1%2Bm_2)l_1%5E2p_2%0A%20%20%20%20%5Cend%7Bpmatrix%7D%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%24%5Cvec%7Bp%7D%5Cequiv(p_1%2Cp_2)%5ET%24%E3%81%AB%E5%AF%BE%E3%81%99%E3%82%8B%E6%AD%A3%E6%BA%96%E6%96%B9%E7%A8%8B%E5%BC%8F%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cdot%7B%5Cvec%7Bp%7D%7D%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20-m_2l_1l_2%5Csin(%5Ctheta_1-%5Ctheta_2)%5Cdot%7B%5Ctheta_1%7D%5Cdot%7B%5Ctheta_2%7D-(m_1%2Bm_2)gl_1%5Csin(%5Ctheta_1)%20%5C%5C%0A%20%20%20%20m_2l_1l_2%5Csin(%5Ctheta_1-%5Ctheta_2)%5Cdot%7B%5Ctheta_1%7D%5Cdot%7B%5Ctheta_2%7D-m_2gl_2%5Csin(%5Ctheta_2)%20%5C%5C%0A%20%20%20%20%5Cend%7Bpmatrix%7D%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(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%E7%B0%A1%E4%BE%BF%E3%81%AE%E3%81%9F%E3%82%81%20%24m_1%3Dm_2%3Dl_1%3Dl_2%3D1%24%20%E3%81%A8%E3%81%97%E3%81%A6%E6%AC%A1%E3%82%92%E5%BE%97%E3%82%8B%E3%80%82%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%26%5Cdot%7B%5Cvec%7B%5Ctheta%7D%7D%0A%20%20%20%20%3D%5Cfrac%7B1%7D%7B2-%5Ccos(%5Ctheta_1-%5Ctheta_2)%7D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20p_1-%5Ccos(%5Ctheta_1-%5Ctheta_2)p_2%20%5C%5C%0A%20%20%20%20-%5Ccos(%5Ctheta_1-%5Ctheta_2)%2B2p_2%0A%20%20%20%20%5Cend%7Bpmatrix%7D%20%5C%5C%0A%20%20%20%20%26%5Cdot%7B%5Cvec%7Bp%7D%7D%3D%0A%20%20%20%20%5Cbegin%7Bpmatrix%7D%0A%20%20%20%20-%5Csin(%5Ctheta_1-%5Ctheta_2)%5Cdot%7B%5Ctheta_1%7D%5Cdot%7B%5Ctheta_2%7D-2g%5Csin(%5Ctheta_1)%20%5C%5C%0A%20%20%20%20%5Csin(%5Ctheta_1-%5Ctheta_2)%5Cdot%7B%5Ctheta_1%7D%5Cdot%7B%5Ctheta_2%7D-g%5Csin(%5Ctheta_2)%20%5C%5C%0A%20%20%20%20%5Cend%7Bpmatrix%7D%0A%20%20%20%20%5Cend%7Baligned%7D%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_(np)%3A%0A%20%20%20%20def%20dp_fmap(x%3A%20np.ndarray%2C%20dt%3A%20float)%3A%0A%20%20%20%20%20%20%20%20theta1%2C%20theta2%2C%20p1%2C%20p2%20%3D%20x%0A%20%20%20%20%20%20%20%20cos%20%3D%20np.cos(theta1%20-%20theta2)%0A%20%20%20%20%20%20%20%20sin%20%3D%20np.sin(theta1%20-%20theta2)%0A%0A%20%20%20%20%20%20%20%20d_theta1%20%3D%20(p1%20-%20cos%20*%20p2)%20%2F%20(2-cos)%0A%20%20%20%20%20%20%20%20d_theta2%20%3D%20-cos%20%2B%202%20*%20p2%20%2F%20(2-cos)%0A%0A%20%20%20%20%20%20%20%20g%20%3D%209.8%0A%20%20%20%20%20%20%20%20d_p1%20%3D%20-sin%20*%20d_theta1%20*%20d_theta2%20-%202%20*%20g%20*%20np.sin(theta1)%0A%20%20%20%20%20%20%20%20d_p2%20%3D%20sin%20*%20d_theta1%20*%20d_theta2%20-%20g%20*%20np.sin(theta2)%0A%0A%20%20%20%20%20%20%20%20theta1_next%20%3D%20theta1%20%2B%20d_theta1%20*%20dt%0A%20%20%20%20%20%20%20%20theta2_next%20%3D%20theta2%20%2B%20d_theta2%20*%20dt%0A%20%20%20%20%20%20%20%20p1_next%20%3D%20p1%20%2B%20d_p1%20*%20dt%0A%20%20%20%20%20%20%20%20p2_next%20%3D%20p2%20%2B%20d_p2%20*%20dt%0A%0A%20%20%20%20%20%20%20%20return%20np.array(%5Btheta1_next%2C%20theta2_next%2C%20p1_next%2C%20p2_next%5D)%0A%0A%20%20%20%20def%20dp_hmap(x%3A%20np.ndarray)%3A%0A%20%20%20%20%20%20%20%20%23%20%E9%81%8B%E5%8B%95%E9%87%8F%E3%81%AF%E4%BD%BF%E7%94%A8%E3%81%97%E3%81%AA%E3%81%84%0A%20%20%20%20%20%20%20%20theta1%2C%20theta2%2C%20_%2C%20_%20%3D%20x%0A%0A%20%20%20%20%20%20%20%20x1%20%3D%20np.sin(theta1)%0A%20%20%20%20%20%20%20%20y1%20%3D%20-np.cos(theta1)%0A%20%20%20%20%20%20%20%20x2%20%3D%20x1%20%2B%20np.sin(theta2)%0A%20%20%20%20%20%20%20%20y2%20%3D%20y1%20-%20np.cos(theta2)%0A%0A%20%20%20%20%20%20%20%20return%20np.array(%5Bx1%2C%20y1%2C%20x2%2C%20y2%5D)%0A%0A%20%20%20%20return%20dp_fmap%2C%20dp_hmap%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%20%E8%A6%B3%E6%B8%AC%E3%83%87%E3%83%BC%E3%82%BF%E3%82%92%E7%94%9F%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(dp_fmap%2C%20dp_hmap%2C%20noise_sigma%2C%20np)%3A%0A%20%20%20%20dt_1%20%3D%200.02%0A%20%20%20%20times%20%3D%20np.arange(0%2C%2010%2C%20dt_1)%0A%20%20%20%20x_list%20%3D%20%5Bnp.array(%5Bnp.pi%20%2F%202%2C%20np.pi%20%2F%202%2C%200%2C%200%5D)%5D%0A%20%20%20%20for%20_t%20in%20times%3A%0A%20%20%20%20%20%20%20%20x_list.append(dp_fmap(x_list%5B-1%5D%2C%20dt_1))%0A%20%20%20%20x_list%20%3D%20np.vstack(x_list)%0A%20%20%20%20dp_obs_rng%20%3D%20np.random.default_rng(44)%0A%20%20%20%20dp_obs_noise%20%3D%20dp_obs_rng.normal(0%2C%20noise_sigma%20**%202%2C%20(len(x_list)%2C%204))%0A%20%20%20%20x_list%20%3D%20np.array(%5Bdp_hmap(x)%20for%20x%20in%20x_list%5D)%0A%20%20%20%20obs_list%20%3D%20np.array(%5Bx%20%2B%20dp_obs_noise%5Bi%5D%20for%20i%2C%20x%20in%20enumerate(x_list)%5D)%0A%20%20%20%20return%20dt_1%2C%20obs_list%2C%20x_list%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%20%E5%8F%AF%E8%A6%96%E5%8C%96%E3%80%82%E3%81%BE%E3%81%9A%E3%81%AF%E3%83%91%E3%83%AC%E3%83%83%E3%83%88%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(blue%2C%20orange%2C%20red)%3A%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt_dp%0A%0A%20%20%20%20dp_fig%2C%20dp_ax%20%3D%20plt_dp.subplots()%0A%20%20%20%20dp_ax.set_aspect('equal')%0A%20%20%20%20dp_ax.set_xlim(-2.3%2C%202.3)%0A%20%20%20%20dp_ax.set_ylim(-2.3%2C%201.5)%0A%20%20%20%20dp_traj_plt%2C%20%3D%20dp_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dblue%2C%20label%3D%22Trajectory%22)%0A%20%20%20%20dp_cur_pos_plt%2C%20%3D%20dp_ax.plot(%0A%20%20%20%20%20%20%20%20%5B%5D%2C%20%5B%5D%2C%20c%3Dorange%2C%20marker%3D%22o%22%2C%20markersize%3D5%2C%0A%20%20%20%20%20%20%20%20label%3D%22Current%20Position%22)%0A%20%20%20%20dp_obs_pos_scat%20%3D%20dp_ax.scatter(%5B%5D%2C%20%5B%5D%2C%20s%3D5%2C%20c%3Dred%2C%20label%3D%22Observation%22)%0A%20%20%20%20dp_ax.legend(loc%3D%22upper%20right%22%2C%20fontsize%3D%22x-small%22)%0A%20%20%20%20return%20dp_ax%2C%20dp_cur_pos_plt%2C%20dp_fig%2C%20dp_obs_pos_scat%2C%20dp_traj_plt%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20Ellipse%2C%0A%20%20%20%20artifacts_dir%2C%0A%20%20%20%20dp_ax%2C%0A%20%20%20%20dp_cur_pos_plt%2C%0A%20%20%20%20dp_fig%2C%0A%20%20%20%20dp_obs_pos_scat%2C%0A%20%20%20%20dp_traj_plt%2C%0A%20%20%20%20gif_image%2C%0A%20%20%20%20noise_sigma%2C%0A%20%20%20%20obs_list%2C%0A%20%20%20%20red%2C%0A%20%20%20%20save_gif%2C%0A%20%20%20%20x_list%2C%0A)%3A%0A%20%20%20%20obs_ellipse_3%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dred%2C%20alpha%3D0.1%2C%20animated%3DTrue)%0A%20%20%20%20dp_ax.add_patch(obs_ellipse_3)%0A%0A%20%20%20%20def%20dp_anim_callback(i)%3A%0A%20%20%20%20%20%20%20%20dp_ax.set_title(f'Frame%20%7Bi%7D')%0A%20%20%20%20%20%20%20%20dp_traj_plt.set_data(x_list%5B%3Ai%2C%202%5D%2C%20x_list%5B%3Ai%2C%203%5D)%0A%20%20%20%20%20%20%20%20dp_cur_pos_plt.set_data(%5B0%2C%20*x_list%5Bi%2C%20%5B0%2C%202%5D%5D%5D%2C%20%5B0%2C%20*x_list%5Bi%2C%20%5B1%2C%203%5D%5D%5D)%0A%20%20%20%20%20%20%20%20dp_obs_pos_scat.set_offsets(obs_list%5B%3A%20i%20%2B%201%2C%20%5B2%2C%203%5D%5D)%0A%20%20%20%20%20%20%20%20obs_ellipse_3.set_center(obs_list%5Bi%2C%20%5B2%2C%203%5D%5D)%0A%0A%20%20%20%20skip%20%3D%204%0A%20%20%20%20dp_gif_path%20%3D%20artifacts_dir%20%2F%20%22double_pendulum.gif%22%0A%20%20%20%20print(save_gif(dp_fig%2C%20dp_anim_callback%2C%20range(0%2C%20len(obs_list)%2C%20skip)%2C%20dp_gif_path%2C%20100))%0A%20%20%20%20dp_animation%20%3D%20gif_image(dp_gif_path%2C%20%22%E4%BA%8C%E9%87%8D%E6%8C%AF%E3%82%8A%E5%AD%90%E3%81%AE%E7%9C%9F%E5%80%A4%E3%81%A8%E8%A6%B3%E6%B8%AC%22)%0A%20%20%20%20return%20(dp_animation%2C)%0A%0A%0A%40app.cell%0Adef%20_(dp_animation)%3A%0A%20%20%20%20dp_animation%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%20%23%23%23%20UKF%20by%20filterpy%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%20UKF%20%E3%83%A2%E3%83%87%E3%83%AB%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(dp_fmap%2C%20dp_hmap%2C%20dt_1%2C%20kalman%2C%20noise_sigma%2C%20np%2C%20obs_list)%3A%0A%20%20%20%20_sigma_points%20%3D%20kalman.MerweScaledSigmaPoints(4%2C%20alpha%3D0.1%2C%20beta%3D2.0%2C%20kappa%3D-1)%0A%20%20%20%20kf_3%20%3D%20kalman.UnscentedKalmanFilter(dim_x%3D4%2C%20dim_z%3D4%2C%20dt%3Ddt_1%2C%20hx%3Ddp_hmap%2C%20fx%3Ddp_fmap%2C%20points%3D_sigma_points)%0A%20%20%20%20kf_3.x%20%3D%20obs_list%5B0%2C%20%3A%5D%0A%20%20%20%20kf_3.P%20%3D%20kf_3.P%20*%20noise_sigma%20**%202%0A%20%20%20%20kf_3.R%20%3D%20noise_sigma%20**%202%20*%20np.eye(4)%0A%20%20%20%20kf_3.Q%20%3D%202%20*%20dt_1%20**%202%20*%20np.eye(4)%0A%20%20%20%20return%20(kf_3%2C)%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%20UKF%20%E5%AE%9F%E8%A1%8C%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(dp_hmap%2C%20kf_3%2C%20np%2C%20obs_list)%3A%0A%20%20%20%20kf_states_3%20%3D%20%5Bkf_3.x%5D%0A%20%20%20%20kf_errors_3%20%3D%20%5Bkf_3.P%5D%0A%20%20%20%20for%20_i%20in%20range(0%2C%20obs_list.shape%5B0%5D%20-%201)%3A%0A%20%20%20%20%20%20%20%20kf_3.predict()%0A%20%20%20%20%20%20%20%20kf_3.update(obs_list%5B_i%20%2B%201%2C%20%3A%5D)%0A%20%20%20%20%20%20%20%20dx_dtheta%20%3D%20np.array(%5B%5Bnp.cos(kf_3.x%5B0%5D)%2C%200%5D%2C%20%5Bnp.sin(kf_3.x%5B1%5D)%2C%200%5D%2C%20%5Bnp.cos(kf_3.x%5B0%5D)%2C%20np.cos(kf_3.x%5B1%5D)%5D%2C%20%5Bnp.sin(kf_3.x%5B1%5D)%2C%20np.sin(kf_3.x%5B1%5D)%5D%5D)%0A%20%20%20%20%20%20%20%20kf_states_3.append(dp_hmap(kf_3.x))%0A%20%20%20%20%20%20%20%20kf_errors_3.append(dx_dtheta%20%40%20kf_3.P%5B%3A2%2C%20%3A2%5D%20%40%20dx_dtheta.T)%0A%20%20%20%20kf_states_3%20%3D%20np.vstack(kf_states_3)%0A%20%20%20%20kf_errors_3%20%3D%20np.dstack(kf_errors_3)%0A%20%20%20%20return%20kf_errors_3%2C%20kf_states_3%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%20%E7%B5%90%E6%9E%9C%E3%81%AE%E6%8F%8F%E7%94%BB%E3%80%82%E3%81%BE%E3%81%9A%E3%81%AF%E3%83%91%E3%83%AC%E3%83%83%E3%83%88%E4%BD%9C%E6%88%90%E3%80%82%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt_dp_ukf%0A%0A%20%20%20%20dp_ukf_fig%2C%20dp_ukf_ax%20%3D%20plt_dp_ukf.subplots()%0A%20%20%20%20dp_ukf_ax.set_aspect('equal')%0A%0A%20%20%20%20%23%20%E6%8F%8F%E7%94%BB%E7%AF%84%E5%9B%B2%0A%20%20%20%20dp_ukf_ax.set_xlim(-2.3%2C%202.3)%0A%20%20%20%20dp_ukf_ax.set_ylim(-2.3%2C%202)%0A%20%20%20%20return%20dp_ukf_ax%2C%20dp_ukf_fig%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%20%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(blue%2C%20dp_ukf_ax%2C%20green%2C%20orange%2C%20red)%3A%0A%20%20%20%20%23%20%E8%BB%8C%E8%B7%A1%E6%8F%8F%E7%94%BB%E3%81%AE%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%0A%20%20%20%20gt_traj_plt_3%2C%20%3D%20dp_ukf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dblue%2C%20label%3D'GT%20Trajectory')%0A%20%20%20%20gt_pos_plt_3%2C%20%3D%20dp_ukf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dorange%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'True%20Point')%0A%20%20%20%20obs_pos_scat_3%20%3D%20dp_ukf_ax.scatter(%5B%5D%2C%20%5B%5D%2C%20s%3D5%2C%20c%3Dred%2C%20label%3D'Observation')%0A%20%20%20%20kf_pos_plt_3%2C%20%3D%20dp_ukf_ax.plot(%5B%5D%2C%20%5B%5D%2C%20c%3Dgreen%2C%20marker%3D'o'%2C%20markersize%3D5%2C%20label%3D'Kalman%20Filter')%0A%20%20%20%20return%20gt_pos_plt_3%2C%20gt_traj_plt_3%2C%20kf_pos_plt_3%2C%20obs_pos_scat_3%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%20%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%E6%8F%8F%E7%94%BB%E3%81%AE%E3%83%AC%E3%82%A4%E3%83%A4%E3%83%BC%E4%BD%9C%E6%88%90%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Ellipse%2C%20dp_ukf_ax%2C%20green%2C%20noise_sigma%2C%20red)%3A%0A%20%20%20%20%23%20%E8%A6%B3%E6%B8%AC%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20obs_ellipse_4%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dred%2C%20alpha%3D0.1%2C%20animated%3DTrue)%0A%20%20%20%20dp_ukf_ax.add_patch(obs_ellipse_4)%0A%20%20%20%20kalman_ellipse_3%20%3D%20Ellipse(xy%3D(0%2C%201)%2C%20width%3Dnoise_sigma%2C%20height%3Dnoise_sigma%2C%20angle%3D0%2C%20color%3Dgreen%2C%20alpha%3D0.3%2C%20animated%3DTrue)%0A%20%20%20%20%23%20%E6%8E%A8%E5%AE%9A%E8%AA%A4%E5%B7%AE%E7%AF%84%E5%9B%B2%0A%20%20%20%20dp_ukf_ax.add_patch(kalman_ellipse_3)%0A%20%20%20%20return%20kalman_ellipse_3%2C%20obs_ellipse_4%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20artifacts_dir%2C%0A%20%20%20%20cov2ellipse%2C%0A%20%20%20%20dp_ukf_ax%2C%0A%20%20%20%20dp_ukf_fig%2C%0A%20%20%20%20gif_image%2C%0A%20%20%20%20gt_pos_plt_3%2C%0A%20%20%20%20gt_traj_plt_3%2C%0A%20%20%20%20kalman_ellipse_3%2C%0A%20%20%20%20kf_errors_3%2C%0A%20%20%20%20kf_pos_plt_3%2C%0A%20%20%20%20kf_states_3%2C%0A%20%20%20%20obs_ellipse_4%2C%0A%20%20%20%20obs_list%2C%0A%20%20%20%20obs_pos_scat_3%2C%0A%20%20%20%20save_gif%2C%0A%20%20%20%20x_list%2C%0A)%3A%0A%20%20%20%20dp_ukf_ax.legend(loc%3D'upper%20right'%2C%20fontsize%3D'x-small')%0A%0A%20%20%20%20def%20dp_ukf_anim_callback(i)%3A%0A%20%20%20%20%20%20%20%20dp_ukf_ax.set_title(f'Frame%20%7Bi%7D')%0A%20%20%20%20%20%20%20%20gt_traj_plt_3.set_data(x_list%5B%3A%20i%20%2B%201%2C%202%5D%2C%20x_list%5B%3A%20i%20%2B%201%2C%203%5D)%0A%20%20%20%20%20%20%20%20gt_pos_plt_3.set_data(%5B0%2C%20*x_list%5Bi%2C%20%5B0%2C%202%5D%5D%5D%2C%20%5B0%2C%20*x_list%5Bi%2C%20%5B1%2C%203%5D%5D%5D)%0A%20%20%20%20%20%20%20%20obs_pos_scat_3.set_offsets(obs_list%5B%3A%20i%20%2B%201%2C%20%5B2%2C%203%5D%5D)%0A%20%20%20%20%20%20%20%20obs_ellipse_4.set_center(obs_list%5Bi%2C%20%5B2%2C%203%5D%5D)%0A%20%20%20%20%20%20%20%20kf_pos_plt_3.set_data(%5Bkf_states_3%5Bi%2C%202%5D%5D%2C%20%5Bkf_states_3%5Bi%2C%203%5D%5D)%0A%20%20%20%20%20%20%20%20width%2C%20height%2C%20angle%20%3D%20cov2ellipse(kf_errors_3%5B2%3A%2C%202%3A%2C%20i%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse_3.set_center(%5Bkf_states_3%5Bi%2C%202%5D%2C%20kf_states_3%5Bi%2C%203%5D%5D)%0A%20%20%20%20%20%20%20%20kalman_ellipse_3.set_width(width)%0A%20%20%20%20%20%20%20%20kalman_ellipse_3.set_height(height)%0A%20%20%20%20%20%20%20%20kalman_ellipse_3.set_angle(angle)%0A%0A%20%20%20%20dp_ukf_skip%20%3D%204%0A%20%20%20%20dp_ukf_gif_path%20%3D%20artifacts_dir%20%2F%20%22double_pendulum_ukf.gif%22%0A%20%20%20%20print(%0A%20%20%20%20%20%20%20%20save_gif(%0A%20%20%20%20%20%20%20%20%20%20%20%20dp_ukf_fig%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20dp_ukf_anim_callback%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20range(0%2C%20len(obs_list)%2C%20dp_ukf_skip)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20dp_ukf_gif_path%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20100%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20)%0A%20%20%20%20dp_ukf_animation%20%3D%20gif_image(dp_ukf_gif_path%2C%20%22UKF%20%E3%81%AB%E3%82%88%E3%82%8B%E4%BA%8C%E9%87%8D%E6%8C%AF%E3%82%8A%E5%AD%90%E3%81%AE%E6%8E%A8%E5%AE%9A%22)%0A%20%20%20%20return%20(dp_ukf_animation%2C)%0A%0A%0A%40app.cell%0Adef%20_(dp_ukf_animation)%3A%0A%20%20%20%20dp_ukf_animation%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