Initialize the simulation
Δt:=10robot:={x:45, y:15, θ:0}μ:=[55
25
0.4
]u:={v:1, ω:0}camera:={x:140, y:12}Σ:=[100 0 0
0 100 0
0 0 0.15
]Q:=[0.01 0
0 0.0025
]R:=[0.25
]θ:=μ[3]Gt:=[1 0 0
0 1 0
-u.v*math/sin(θ)*Δt u.v*math/cos(θ)*Δt 1
]Vt:=[math/cos(θ)*Δt math/sin(θ)*Δt 0
0 0 Δt
]μ2:=μ+[u.v
u.v
u.ω
]*[math/cos(θ)
math/sin(θ)
1
]*ΔtΣ2:=Gt**Σ**Gt'+Vt**Q**Vt'Δy:=camera.y-μ[2]Δx:=camera.x-μ[1]q:=Δx^2+Δy^2Ẑ:=math/atan2(Δy, Δx)-θH:=[Δy/q
-Δx/q
-1
]S:=H**Σ**H'+RK:=Σ**H'/S[1]μ3:=μ'+K*ẐΣ3:=([1 0 0
0 1 0
0 0 1
]-K**H)**Σ2