姿态解算

姿态解算全过程

关于这方面，姿态计算的理解大致需要经过以下几个步骤。

1.秦永元的《惯性导航》，不但十分基础而且写的也十分好，适合入门。但是并不是所有章节都是需要看的，
  其中1.2节 9.2节和9.5节是必须看的，而且是要必须深入理解和推导的，其他的看不看都行，仅限于个人理解。
2.看完秦永元的书基本上有了个大概理解，再看“2.四元数”就有点基础了，这个也不是全看。那上面说的旋转
  其实是个数学概念，我们的目的不是去了解旋转，只是了解下四元数，把数学当做一种工具，毕竟是工程应用中的数学。
3.“3.万向节锁死”这个万向节锁死我至今没明白，而且也不想去明白了，只知道直接使用欧拉角解算姿态会有个万向节
  锁死的弊端，有需要的可以去找资料了解一下。
4.线性代数基础要好，因为一个向量是可以写成矩阵形式的（我之前也忘了，后来突然想起来考研时张宇讲过3*3方阵线性
  相关和线性无关就是用的向量来讲的，当时还觉得好奇怪，因为大家都吐槽张宇代数讲的不好。果断在这里为宇哥正名），
  尤其是上面进行坐标系的变换就是出自代数向量空间里的内容，B = P逆 * A ，这个是坐标变换公式，要熟记。P在代数里
  成为过渡矩阵。而在姿态计算里面称为变换矩阵或者是DCM（方向余弦矩阵），个人觉得没啥区别。而且在姿态解算代码中
  也有重力加速度的从地理坐标系变换到机体坐标系使用的就是上面这个公式。
5.然后的话看看这个视频，这个讲的也蛮好的。
  https://www.bilibili.com/video/av13035245/?spm_id_from=333.338.recommend_report.5
  这位明显是大佬，首先理论搞得十分清楚，其次代码讲解也很强
6.配合着上面这个视频，还可以看下这几个博文，基本上姿态解算就差不多了，先学会一种方法（Mahony的互补滤波算法），
  然后先飞起来等了解的差不多了再去了解一些其他的方法（梯度下降法之类的）。
  链接如下
  http://www.crazepony.com/book/about-algorithm.html  这个是Crazypony的 WIKI，关于姿态这块讲的还行。
  http://blog.sina.com.cn/s/blog_d0b80cdd0102x2gt.html
  https://blog.csdn.net/wx601056818/article/details/81412216   看完河南理工那个视频再看这个挺好的 
  https://blog.csdn.net/wubaobao1993/article/details/53130362  这个代码和河南理工那个也是差不多的
  https://blog.csdn.net/AASDSADAD/article/details/73080832?utm_source=blogxgwz8  当初不知道什么是龙格库塔法..
  https://blog.csdn.net/qq_21842557/article/details/50884695   这个是自动控制专业的大佬学的Pixhawk笔记，可以参考
  
基本上看完这些，对姿态解算大概都会有一些理解了。




/*****************************************************************************************************************************/
关于姿态解算这块今天算是初窥门径。之前也是看过学过，但是没有动力，没看懂或者是有其他事情就都放过去了，
今天着实感觉算是稍微了解了一点，简单谈下我的看法，虽然网上有很多教程，我也看了很多，但是我还是感觉
不全，比较零碎。在次我首推秦永元的《惯性导航》，简直不要太棒。

先从整体上说下关于 
坐标系转换
四元数
欧拉角
方向余弦
IMU
转换矩阵
等之间的关系
 
首先是坐标系转换，了解过的朋友们都知道有两个坐标系，地理坐标系n和机体坐标系b，但是为什么要进行坐标系转换呢？
很显然，我们所处的位置是地理坐标系，而四轴在飞行过程中对于四轴来说是机体坐标系，举个例子，我们想要飞行器拐弯，
但是飞行器怎么知道我们是让它拐弯呢？很明显是将两个坐标系联系在一起。还有一个就是有头模式与无头模式的区别，
讲的就是让飞行器处于哪个坐标系下飞行。

然后呢，我们可以想起来线性代数课本上讲过，基变换与坐标变换，很明显，第二种是我们需要的，也有公式
( A | B ) -> ( E | P ),B是新的，A是旧的，我们可以求出P过渡矩阵，而坐标变换公式则为 B = (P-1) * A ， (P-1)为P的逆
由于不好表达所以简代，这两个公式中A B 不是一样的，但A都表示旧的，B表示新的，后面这个公式讲的是已知在
原坐标系中的坐标，求新坐标系中的坐标。而第一个公式是用来求过渡矩阵P的。自然在秦永元的《惯性导航》中
我也见到了类似的表达，其实是一样的，进行坐标变换都时要遵循一定的规则，翻开前面的基础知识不难看到
R2 = C * R1，显然，这里C和(P-1)是一致的，那么因此，我们就接着往下看。提下，代数里面的过渡矩阵P和转换矩阵
是一个意思。

然后秦永元里面还有句话，姿态更新是指根据惯性器件的输出实时计算出C矩阵。仔细读三遍。发现这一句话包含三个概念。
我之前还在考虑，如果只是为了求出转换矩阵C，那么IMU进行数据采集到底是为了什么？原来IMU的数据采集是为了
计算出转换矩阵C，这就叫姿态解算。

但是在进行转换矩阵C的求解过程中，又出现了四元数 欧拉角和方向余弦等用来表示转换矩阵C的方法，其中首推四元数，
它详细的包含了姿态解算中的全部信息。但是其他两个也都有自己的优点，相对而言在MCU上还是比较适合使用
四元数的。但是我们的目的并不是为了求出转换矩阵C，由四元数自然可以表示出转换矩阵，相应的，我们也可以求出
三个欧拉角，俯仰角，滚转角和偏航角。

目前就先了解到这里，可能解释的比较笼统，还是十分推荐秦永元的那本《惯性导航》。书读百遍，其义自见。

/*****************************************************************************************************************************/

龙格库塔法 就是用来解出四元数的一种途径。
是用于非线性常微分方程的解的重要的一类隐式或显式迭代法
在工程上应用广泛，且精度较高。
详情请见，基本上搞不定，工程数学大致懂原理使用结果即可

https://blog.csdn.net/AASDSADAD/article/details/73080832?utm_source=blogxgwz8

/*****************************************************************************************************************************/


又有新感受
我理解东西比较喜欢抓逻辑，先从整体上理解，而后抓细节。
我们为什么要进行姿态解算？
怎样进行姿态解算？

1.明确一个概念，我们所说的姿态解算不过是进行姿态检测而已，就是利用传感器得出我们所需要的信息。
  而后还有姿态控制，PID当仁不让。而姿态检测就是为了为姿态控制提供足够的信息。
  所以首先给自己定位，处在姿态检测这个阶段，姿态控制后面再说。

2.我们姿态解算的目的是什么？
  就是为了得出三个欧拉角，pitch roll yaw，因为姿态控制的输入只能是三个欧拉角。
（来自Crazypony,由于本人还没做到控制所以也不是很清楚，不过给自己找个理论支撑是必要的，哪怕之后颠覆重建）
  真搞不懂有些人一直叫嚷着姿态解算姿态解算，但是最后对自己想要得出什么东西一无所知，连主次都分不清，真可怕。

3.为什么要进行坐标系转换？
  显然，地理坐标系和机体坐标系两者明显不同。我们处在地理坐标系中而无人机处在机体坐标系中，都是相对的。
  很简单如果我们想让无人机往前飞，但是无人机怎么知道是让它往前飞呢？不妨将机体坐标系变换到地理坐标系中，
  明显需要将两个坐标系联系在一起。
  
4.欧拉角，四元数，正交余弦矩阵三者有什么关系？为什么都说使用四元数更简便？
  事实上三者都是一样的，只不过是不同的表达形式而已，哪种适合用那种。
  至于网上都说使用四元数来计算更简便，我的看法是：其实这三种都使用到了，在坐标变换过程中使用了DCM，然后
  DCM可以用欧拉角来表示，也可以用四元数来表示，而我们最后的需求也是欧拉角。但是DCM在推导过程中起到了
  很大的作用，尤其是对帮助理解很有作用。四元数不过是在代码中应用比较方便而已，而欧拉角作为姿态控制的入口参数
  也很重要。也就是说，四元数不过是起过渡的作用，也没有那么神秘。
 
5.姿态解算的大致过程是什么？
  目的即是为了求出欧拉角，首先进行坐标系变换，而转换矩阵就将两者联系在一起，而由理论可以推导出转换矩阵可以由
  欧拉角来表示，同样由理论，转换矩阵也可以由四元数来表示，由于四元数在代码中使用比较方便，因此我们可以求出四元数，
  而求解四元数也有一定的工程方法，最后由四元数和欧拉角的关系解出欧拉角。

6.基于Mahony的互补滤波算法挺有意思，可以好好研究下，先飞起来，再改进研究其他的。

/*****************************************************************************************************************************/