deadreckonspace.m

来自「该程序用PID系统实现差动转向车辆控制」· M 代码 · 共 16 行

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

function new_position=deadReckonSpace(position,sr,sl,bw)
%% This function calculate the new position of the robot
%% according to the space covered by the right wheel and by the left wheel
x0=position(1);
y0=position(2);
theta0=position(3);
theta=theta0+(sr-sl)/bw;
x=x0+(sr+sl)/2*(cos(theta));
y=y0+(sr+sl)/2*(sin(theta));
new_position=[x,y,theta];

end