通俗一点描述惯导：
火箭上装有   加速度计   和   角速率陀螺仪，先测出火箭方位（倾角）。
再把 加速度 折合成地面观察到的数值，积分后得速度和位移。

[新增]有奖问答(1000kcb)：
转动四元数r按原公式（倒数第三幅图）计算，出了什么问题？
提示：化简公式r = [1 thetax/2 thetay/2 thetaz/2 ] 没有这个问题。

现在，我们来讲讲具体实现：

坐标系定义


信号流

关键就在T(t,b)这个矩阵，它可以把火箭上的加速度转换成地面观测到的加速度。
借助四元数这个桥梁，可以大大简化计算。


注意加速度变换的方法，一个矩阵乘法搞定。

上面的四元数Q怎么得到的呢？看这里

  

四元数需要规范化处理，以保证方向余弦矩阵是一个正交矩阵
可以理解为保持数学平台（也就是方向余弦矩阵）3轴是垂直的，
平台惯导通过准确的加工安装来保证正交性


到此已经解决了忽略地球自转的情况下，解算运动参数的问题。
还有3个主要问题：
1.如何得到姿态角？
2.如何确定四元数初始值？（也就是确定火箭初始方位）
3.考虑地球自转
精彩待续

附演示代码：
matlab.rar 382.61KB RAR 132次下载
先导入数据文件（mat格式，双击导入），再运行instest.m