计算非等间隔离散曲线的曲率
简介
由于需要计算车辆行驶轨迹的曲率,但由于车辆的车速不同,每次采集的轨迹点非等间隔,这里借鉴了诸多大神的经验,主要参考了下面的思路才得到最后结果。
参考:link.
代码
function Curvature = ThreePoint2Curvature(x,y,n)
x = reshape(x,3,1);
y = reshape(y,3,1);
t_a = norm([x(2)-x(1),y(2)-y(1)]);
t_b = norm([x(3)-x(2),y(3)-y(2)]);
M = [1, -t_a, t_a^2;
1