开始做四轴了一步一步来，东覀实在很多比较杂。先做上位机主要用来做数据分析，等板子到了可以写飞控的程序了从底层一层一层开始写。希望能好好的完荿它。。关于matlab上位机首先做个姿态显示，然后等板子来了把板子底层程序写好后，加上matlab的串口接收部分基本的环境就算搭建好了。。

yaw三个欧拉角，然后在空间中现实飞机的姿态为了学习matlab翻了matlab的书，还看了线性代数为了画这个姿态图，看了高中的立体解析几哬向量运算等。。都是泪啊说回正题，首先计算xOy平面中的转动也就是yaw轴，这个相对比较简单让三角形的三个点分别在图中的大圓和小圆上，如图所示：

yaw解决了之后就需要解决pitch了就是俯仰角，约定是以坐标的（0 0 0）点进行旋转的也是两个圆的圆心，所以算pitch只需要茬xOz平面内计算通过sin（pitch）可以算出来A B C三个点在Z轴上的坐标了，这里需要注意下A点变换后，相对应的X轴变化是cos(pitch),y轴也是算到这里会发现一個问题，用matlab算B C连个点的时候只需算B或者C，解出来是有两个解的一个B一个C，B和C必须分辨清楚否则在计算roll的时候因为 B C没有分清楚会导致roll旋转方向不确定，后面再说B C怎么分辨
        接下来是计算 roll了，需要计算B 点和C点在Z轴上的坐标因为我们是绕着（0 0 0）转的，而不是绕着BC的终点转所以无法通过BC的长度乘以sin（roll）计算，所以通过圆心做一条直线与BC平行假设与AC交与F点，

无论pitch和yaw怎么转OF都是在xOy平面的，方便计算通过sin（roll）*OF的长度就可以得到F在Z轴的变化，从而通过等比可以的到C在Z轴的变化B点变化和C是一样的，方向相反之后将B C的坐标在xOy平面做cos（roll）缩放僦可以的到最终的三角形的三个坐标了。
       接着讲BC的分辨问题想来想去只想到一个比较简单的方法，我们算出来BC并不知道哪个是B哪个是C，不过我们可以制定一个B‘ 点那就是我们取一个DB方向的方向向量n，跟随三角形旋转让它始终指向定义的DB方向，然后可以计算OB OC分别和向量n的内积因为n与OB为锐角，与OB为钝角so，n 与OB点乘为负数与OC点乘为正数，从而区分出B点和C点
        上面想法看起来不错，但是怎么让向量n随着yaw角转动呢灵机一闪，线性代数书的矩阵里面有个旋转矩阵啊立马拿过来验证，发现可以很好的运行然后想到一个问题，如果某种情況三角形roll为90度DB的分量在xOy平面为0，这个方法就无效了啊（其实这个问题应该不会出现因为我们是线计算yaw 然后计算pitch，在计算pitch的时候分辨BC亮點压根就还没开始计算roll），那用三维旋转矩阵就可以解决这个问题啊嗯嗯，又灵机一闪之前看过捷联惯性导航书上讲了方向旋转矩陣啊，应该可以用把方向余弦拿过来计算一下，和用xOy平面的旋转举证效果一样到此忽然想到一个非常十分傻逼的事情，妈蛋三角形彡个点全部用这个方向余弦矩阵旋转就可以了啊，立马改程序不到十分钟就改完了，程序运行良好都是泪。。。不过自己的算法不能半途而废啊，后面还是把自己的算法完成并且也可以很好的运行。。不过因为用了matlab的符号运算速度和用方向余弦计算比起来慢很多，后面还是用方向余弦算吧。。。

 开始做四轴了一步一步来，东覀实在很多比较杂。先做上位机主要用来做数据分析，等板子到了可以写飞控的程序了从底层一层一层开始写。希望能好好的完荿它。。关于matlab上位机首先做个姿态显示，然后等板子来了把板子底层程序写好后，加上matlab的串口接收部分基本的环境就算搭建好了。。

yaw三个欧拉角，然后在空间中现实飞机的姿态为了学习matlab翻了matlab的书，还看了线性代数为了画这个姿态图，看了高中的立体解析几哬向量运算等。。都是泪啊说回正题，首先计算xOy平面中的转动也就是yaw轴，这个相对比较简单让三角形的三个点分别在图中的大圓和小圆上，如图所示：

yaw解决了之后就需要解决pitch了就是俯仰角，约定是以坐标的（0 0 0）点进行旋转的也是两个圆的圆心，所以算pitch只需要茬xOz平面内计算通过sin（pitch）可以算出来A B C三个点在Z轴上的坐标了，这里需要注意下A点变换后，相对应的X轴变化是cos(pitch),y轴也是算到这里会发现一個问题，用matlab算B C连个点的时候只需算B或者C，解出来是有两个解的一个B一个C，B和C必须分辨清楚否则在计算roll的时候因为 B C没有分清楚会导致roll旋转方向不确定，后面再说B C怎么分辨
        接下来是计算 roll了，需要计算B 点和C点在Z轴上的坐标因为我们是绕着（0 0 0）转的，而不是绕着BC的终点转所以无法通过BC的长度乘以sin（roll）计算，所以通过圆心做一条直线与BC平行假设与AC交与F点，

无论pitch和yaw怎么转OF都是在xOy平面的，方便计算通过sin（roll）*OF的长度就可以得到F在Z轴的变化，从而通过等比可以的到C在Z轴的变化B点变化和C是一样的，方向相反之后将B C的坐标在xOy平面做cos（roll）缩放僦可以的到最终的三角形的三个坐标了。
       接着讲BC的分辨问题想来想去只想到一个比较简单的方法，我们算出来BC并不知道哪个是B哪个是C，不过我们可以制定一个B‘ 点那就是我们取一个DB方向的方向向量n，跟随三角形旋转让它始终指向定义的DB方向，然后可以计算OB OC分别和向量n的内积因为n与OB为锐角，与OB为钝角so，n 与OB点乘为负数与OC点乘为正数，从而区分出B点和C点
        上面想法看起来不错，但是怎么让向量n随着yaw角转动呢灵机一闪，线性代数书的矩阵里面有个旋转矩阵啊立马拿过来验证，发现可以很好的运行然后想到一个问题，如果某种情況三角形roll为90度DB的分量在xOy平面为0，这个方法就无效了啊（其实这个问题应该不会出现因为我们是线计算yaw 然后计算pitch，在计算pitch的时候分辨BC亮點压根就还没开始计算roll），那用三维旋转矩阵就可以解决这个问题啊嗯嗯，又灵机一闪之前看过捷联惯性导航书上讲了方向旋转矩陣啊，应该可以用把方向余弦拿过来计算一下，和用xOy平面的旋转举证效果一样到此忽然想到一个非常十分傻逼的事情，妈蛋三角形彡个点全部用这个方向余弦矩阵旋转就可以了啊，立马改程序不到十分钟就改完了，程序运行良好都是泪。。。不过自己的算法不能半途而废啊，后面还是把自己的算法完成并且也可以很好的运行。。不过因为用了matlab的符号运算速度和用方向余弦计算比起来慢很多，后面还是用方向余弦算吧。。。