基于码盘和JY901的全场定位

1、用JY901可以读出Z轴方向的偏航角。
不用再做姿态解算，内置卡尔曼滤波。 使用前进行校准。

2、用编码器读出脉冲数、进而算出位移

3、通过位移和偏航角算出位置

xy = sqrt{ (x2 + y2) } 通过左轮和右轮求出位移

posion_x = posion_x + xycos(（θ1 + θ2）/ 2)
根据上一时刻的x位置求出当前时刻的位置
posion_y= posion_y + xycos(（θ1 + θ2）/ 2)
根据上一时刻的y位置求出当前时刻的位置

北科的全场定位系统的算法和实现：
https://www.docin.com/p-99357443.html