自走車線軌跡的曲率偵測 Curvature calculation in Robotrace contests

假設自走車的直線速度是 $v_C$ 而且被控制為定值，角速度是 $\omega_C$，重心位置與前方紅外線感測器用來估測線軌跡誤差控制中心點的距離為 $L$。 那麼控制中心點的速度 $v_{LC}$ 的大小，以及它與自走車直線速度方向的夾角為 $\phi$，可以用下列的方程式算出來 $$v^2_{LC} = v^2_C + (\omega_C L)^2$$ $$\phi = \tan^{-1}\left(\dfrac{\omega_C L}{v_C}\right)$$
假設我們開始觀測的時間為 $t_0$，而且自走車的直線速度 $v_C$ 在 $t_0$ 到 $t$ 時間內的角度變化為 $\theta(t)-\theta(t_0)$。其中 $$\theta(t) - \theta(t_0) = \int^t_{t_0} \omega_C(\tau) d\tau$$  
那麼控制中心點速度 $v_{LC}$ 在 $t_0$ 到 $t$ 時間內的角度變化 $\alpha$ 就是 $$\alpha = \theta(t)-\theta(t_0) + \tan^{-1}\left(\dfrac{\omega_C(t) L}{v_C}\right) - \tan^{-1}\left(\dfrac{\omega_C(t_0) L}{v_C}\right)$$
如果控制中心點能夠一直沿著線軌跡沒有誤差的移動，那麼在時間 $t$ 時線軌跡的曲率半徑 $r$ 就可以利用下列方程式來估測 $$r = \dfrac{l}{\alpha}$$ $$l = \int^t_{t_0} v_{LC}(\tau) d\tau$$
 另一個估測的方法是 $$r = \dfrac{v_{LC}}{\dot{\alpha}}$$ $$\dot{\alpha} = \dfrac{d\alpha}{dt} = \omega_C + \dfrac{Lv_C}{v^2_C+(\omega_C L)^2} \dfrac{d\omega_C}{dt}$$
 這是因為 $$\dfrac{d\tan^{-1}(x)}{dx} = \dfrac{1}{1+x^2}$$ 但是當控制中心點無法一直沿著線軌跡沒有誤差的移動時，該怎麼修正呢？ 目前的想法是 $$r_t = r - \delta x$$ 其中 $r_t$ 是修正後的估測半徑，$\delta x$ 是控制中心點與線軌跡間的誤差大小。