惯性导航扩展卡尔曼滤波MATLAB

惯性导航扩展卡尔曼滤波（MATLAB）

function q = a2quat（att）
% 先求姿态矩阵
si = sin（att（1））; ci = cos（att（1））; sj = sin（att（2））; cj = cos（att（2））;
sk = sin（att（3））; ck = cos（att（3））;
M = [ cj*ck+si*sj*sk ci*sk sj*ck-si*cj*sk;
-cj*sk+si*sj*ck ci*ck -sj*sk-si*cj*ck;
-ci*sj si ci*cj ];
% 再求四元数
q = [ sqrt（abs（1.0 + M（11） + M（22） + M（33）））/2.0;
sign（M（32）-M（23）） * sqrt（abs（1.0 + M（11） - M（22） - M（33）））/2.0;
sign（M（13）-M（31）） * sqrt（abs（1.0 - M（11） + M（22） - M（33）））/2.0;
sign（M（21）-M（12）） * sqrt（abs（1.0 - M（11） - M（22） + M（33）））/2.0 ];

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        540  2018-04-09 16:02  QEKF1\a2qua.m

     文件       4151  2019-02-25 21:26  QEKF1\attitude.m

     文件        400  2018-06-01 12:54  QEKF1\cnscl.m

     文件        397  2018-06-01 12:54  QEKF1\earth.m

     文件       1574  2019-02-25 22:00  QEKF1\glvs.m

     文件     403902  2019-05-16 20:33  QEKF1\imu_data\imu_data.txt

     文件       1244  2019-02-25 22:03  QEKF1\kfinit0.m

     文件      20178  2019-05-16 20:33  QEKF1\mag_acc_data\mag_acc_data.txt

     文件        652  2018-06-01 12:54  QEKF1\q2att.m

     文件        104  2018-04-09 16:01  QEKF1\q2rv.m

     文件         48  2018-04-09 15:57  QEKF1\qconj.m

     文件        978  2019-02-25 22:02  QEKF1\QEAHRSInit.m

     文件       1956  2019-02-27 21:05  QEKF1\QEAHRSUpdate.m

     文件       5804  2019-05-28 22:17  QEKF1\QEKF1.m

     文件        288  2018-04-09 15:58  QEKF1\qmul.m

     文件         90  2018-04-12 16:12  QEKF1\qmulv.m

     文件        416  2014-08-11 11:03  QEKF1\qnormlz.m

     文件        133  2018-04-09 16:00  QEKF1\rv2q.m

     文件        210  2019-05-15 19:38  QEKF1\save_data.m

     文件        573  2018-06-01 12:55  QEKF1\sins.m

     文件         29  2019-05-19 16:41  QEKF1\test1.m

     文件       1000  2018-09-15 12:24  QEKF1\test2.m

     文件         90  2019-04-28 22:12  QEKF1\test3.m

     文件        821  2019-05-15 19:23  QEKF1\usart3.m

     文件       2681  2019-05-15 19:32  QEKF1\usart3_callback.m

     目录          0  2019-05-20 13:54  QEKF1\imu_data

     目录          0  2019-05-20 13:54  QEKF1\mag_acc_data

     目录          0  2019-05-28 22:21  QEKF1

----------- ---------  ---------- -----  ----

               448259                    28

............此处省略1个文件信息