GPS部分
GPS_angle[ROLL] = (nav[LON]*cos_yaw_x - nav[LAT]*sin_yaw_y) /10;
GPS_angle[PITCH] = (nav[LON]*sin_yaw_y + nav[LAT]*cos_yaw_x) /10;
这是二维向量的旋转，导航点的坐标是相对飞机在正前方 也就是0度时，当飞机Yaw旋转一定角度时，实际的导航点角度应逆时针旋转飞机的角度。
原文：http://www.cnblogs.com/zwb800/p/4451432.html