记车辆状态量 q = ( x , y , θ , v , κ , a ) q=(x,y,\theta,v,\kappa,a) q=(x,y,θ,v,κ,a)
1、观测值计算
根据以下公式通过车辆x，y计算得到第i时刻的状态量的观测值，其中theta和v需要两帧数据，k和a需要三帧数据，k和a在无计算值时可先设为0：
θ i = a t a n ( y i − 1 − y i x i − 1 − x i ) v i = ( x i − 1 − x i ) 2 + ( y i − 1 − y i ) 2 d t k i = θ i − 1 − θ i ( x i − 1 − x i ) 2 + ( y i − 1 − y i ) 2 a i = v i − 1 − v i d t \begin{aligned} \theta_i &=atan(\frac{y_{i-1}-y_i}{x_{i-1}-x_i}) \\ v_i&=\frac{\sqrt{(x_{i-1}-x_i)^2+(y_{i-1}-y_i)^2}}{dt}\\ k_i&= \frac{\theta_{i-1}-\theta_i}{\sqrt{(x_{i-1}-x_i)^2+(y_{i-1}-y_i)^2}}\\ a_i&=\frac{v_{i-1}-v_i}{dt} \end{aligned} θivi
最低0.47元/天 解锁文章
379
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
