Pixhawk之姿态解算篇（1）_入门篇（DCM Nomalize）

一、开篇

        慢慢的、慢慢的、慢慢的就快要到飞控的主要部分了，飞控飞控就是所谓的飞行控制呗，一个是姿态解算一个是姿态控制，解算是解算，控制是控制，各自负责各自的任务，我也不懂，还在学习中~~~~

        最近看姿态估计部分看的太累了，明显发现基础知识太薄弱，什么欧拉角、DCM、四元数、gyro误差、矫正、正交化等各个概念。然后就是各种转换公式，接下来结合代码介绍一些基本的东西。太深入的还不了解~~~

        一定要多看论文啊，英文版的论文（也没有中文的，国人的悲哀啊），虽然看着头疼，看是看完了以后就会发现许多不了解的迷惑的就自然都解开了。

二、版权声明

博主：summer

声明：喝水不忘挖井人，转载请注明出处。

原文地址：http://blog.csdn.net/qq_21842557

联系方式：[email protected]

技术交流QQ：1073811738

三、实验平台

Software Version：ArduCopter（Ver_3.3）

Hardware Version：pixhawk

IDE：eclipse Juno （Windows）

四、基本知识

1、如何实现控制

        一个无人机系统的算法主要有两类：姿态检测算法、姿态控制算法。姿态控制、被控对象、姿态检测三个部分构成一个闭环控制系统。被控对象的模型是由其物理系统决定，设计无人机的算法就是设计姿态控制算法、姿态检测算法。

        1）姿态检测算法：姿态的表示可以用欧拉角，也可以用四元数。姿态检测算法的作用就是将加速度计、陀螺仪等传感器的测量值解算成姿态，进而作为系统的反馈量。在获取sensors值之前需要对数据进行滤波，滤波算法主要是将获取到的陀螺仪和加速度计的数据进行去噪声及融合，得出正确的角度数据（欧拉角或四元数），主要采用互补滤波或者高大上的卡尔曼滤波。

        2）姿态控制算法：控制无人机姿态的三个自由度，用给定姿态与姿态检测算法得出的姿态偏差作为输入，被控对象模型的输入量作为输出（例如姿态增量），从而达到控制无人机姿态的作用。最常用的就是PID控制及其各种PID扩展（分段、模糊等，我的毕设就是模糊PID控制算法，惨了，啥都不懂，还能毕业不，哎~~~），高端点的有自适应控制（自动壁障应该就用这个）。当然，姿态控制算法里面比较常用角速度、角度双闭环控制（所谓的两级PID控制），所以常常有PID外环+PID内环等等，懂我搞懂了再细说吧。PID算法就是控制四个电机的转速来纠正欧拉角，从而使机身保持平稳。

2、关于编译环境的搭建

        ardupilot的编译环境搭建比较简单，直接github下载或者clone到PC上就可以了。PX4Firmware的开发环境的搭建有点困难，结合几人之力中搞定了，现在把大致过程写下来以便帮助更多的人。

        下面主要是讲述关于PX4Firmware开发平台的搭建，即所谓的pixhawk原生固件的开发环境。直接使用github下载PX4Firmware源码到PC上，在toolchain中的console控制台中使用make命令编译即可，在使用命令是需要在Firmware的目录下面才行，不然会出现无效命令的错误提示。编译过程相当复杂耗时，所以慢慢的等吧，如果中途出现编译到某处以后等待了10分钟之久还是没有往下运行，那么关掉控制台重启，重新make即可。在eclipse中编译有点难度，不仅需要安装Cmake；还需要在别的地方配置一二，首先是修改Firmware中的两个文件名，即template_.cproject和template_.project修改为.cproject.和.project.（注意名称前后各一个点“.”），然后就是修改eclipse的环境变量，修改成如下。原本默认的是“E:\PX4\Firmware”，现修改为“E:/PX4/Firmware”,就是斜杠的问题。在eclipse下很容易出现问题，所以建议还是在控制台编译吧，而且有很炫的颜色。

        如果是拷贝别人的源码，可能会出现无法编译的情况，原因不是文件丢失的问题，而是编译一次以后eclipse会默认配置好一些路径，所以，在拷贝的时候按照原本的源文件的目录重新建一个一个的目录就可以了，比如本来是E/PX4目录，那么就在你的电脑中也建一个同样的目录，把拷贝的东西放进去就行了。

        PS：普遍遇到的一个问题就是每次编译都会git submodule update，主要就是因为在使用console控制台命令行编译时的一个执行过程就是Tools中的check_submodules.sh，很明显是shell脚本，这个脚本首先检测源码路径中是否有“.git”，没有的话将无法实现git，然后检查.sh中指定的submodule，没有的这些submodule的话需要联网git，有的话直接跳过，显示Checked xxxx currect version found 。不联网时可以通过不执行/修改这个shell脚本略过检查更新固件，下面会详细介绍这个。

        Tools里面有很多个.sh脚本，也就是一些关于配置的，比如make_color.sh，自己去配置为喜欢的颜色吧，关于控制台颜色的问题详见：http://blog.chinaunix.net/uid-598887-id-4019034.html。

五、正文

1、ardupilot到PX4Firmware的变化

        首先说明一下，这两套代码我是结合着看的，反正ardupilot的底层也是直接调用的PX4Firmware，博文还是以ardupilot为主线展开。介绍一下ardupilot到PX4Firmware的变化吧，其实变化的不多，主要就是由原本的make转变到了现在的Cmake，原来在ardupilot里面处处可以见到.mk，现在是彻底的没有了，全部都是CmakeList.txt和.cmake了，它俩就是由Cmake写的makefile，一看就知道里面是什么意思，不做解释了。

        另外一个比较重要的变化就是关于各种库的选择编译的配置，搬移到了PX4-Firmware/cmake/config中，即nuttx_px4fmu-v2_default.cmake，可以自己修改这个cmake文件增加或删除某个库。该部分会在console控制台中使用命令make编译时调用，在PX4-Firmware/Makefile中有如下代码：

 px4fmu-v2_default:
     $(call cmake-build,nuttx_px4fmu-v2_default)
 cmake-build是宏定义，nuttx_px4fmu-v2_default作为参数传入。
 # Functions
 # --------------------------------------------------------------------
 # describe how to build a cmake config
 define cmake-build
 +@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi
 +@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
 +Tools/check_submodules.sh/*该脚本检查是否需要更新固件源码，如不需可以直接屏蔽*/
 +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
 Endef
 其中：PX4_MAKE = make
       PX4_MAKE_ARGS = -j$(j) --no-print-directory

        解析以后的命令就是如下图所示。

        简要介绍一下Cmake：其实也没什么难的，我们不需要写多高端的，只是会用它即可，几乎每个文件里面都有.cmake，在PX4-Firmware/cmake/中的common或者nuttx中是一些基本的cmake函数（类似于C/C++语言的的库一样）以供在别处编写makefile时使用（比如在PX4-Firmware/src/modules/commander的CMakeLists.txt）。

        举一例说明问题，在PX4-Firmware/cmake/common中的px4_base.cmake，该文件的最前端部分会介绍本file内部有哪些function以及介绍每个function的用法，使用时就按照这里面的example仿写就行。在CMakeLists.txt中不需要使用include（xxx.cmake）就可以使用需要的function，但是在xxx.cmake中使用时需要include(common/px4_base) 以后才可以使用（和C/C++类似）。下面是Firmware/src/modules/commander的CMakeLists.txt代码。

 set(MODULE_CFLAGS -Os)
 if(${OS} STREQUAL "nuttx")            /*判断OS类型*/
     list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450)
 endif()
 px4_add_module(                   /*在px4_base.cmake中定义，里面会介绍用法*/
     MODULE modules__commander   /*标明路径*/
     MAIN commander   /* 类似于.mk中的MODULE_COMMAND = commander*/
     STACK 4096
     COMPILE_FLAGS
         ${MODULE_CFLAGS}
         -Os
     SRCS                          /*source files*/
         commander.cpp
         state_machine_helper.cpp
         commander_helper.cpp
         calibration_routines.cpp
         accelerometer_calibration.cpp
         gyro_calibration.cpp
         mag_calibration.cpp
         baro_calibration.cpp
         rc_calibration.cpp
         airspeed_calibration.cpp
         esc_calibration.cpp
         PreflightCheck.cpp
     DEPENDS
         platforms__common
     )

2、在分析代码之前首先需要了解一下arducopter/copter.h文件，内部以class类的形式定义了几乎所有的使用的函数。平时索引函数时可以先到该文件中查找一下。

3、程序的main入口（补充）

        总的来说，这里的main函数就是ArduCopter.cpp里的AP_HAL_MAIN_CALLBACKS(&copter)，它实际上是一个宏定义，传进来的参数为类对象的引用，通过在AP_HAL_Main.h里的定义可知原型为：

 #define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
     int AP_MAIN(int argc, char* const argv[]); \
 int AP_MAIN(int argc, char* const argv[]) { \

         hal.run(argc, argv, CALLBACKS); \
         return 0; \
     } \
     }

        而这里的AP_MAIN也是一个宏，如下：

 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 #define AP_MAIN __EXPORT ArduPilot_main
 #endif

        解析以后实际上是这样的：

 #define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
     int __EXPORT ArduPilot_main(int argc, char* const argv[]); \
     int __EXPORT ArduPilot_main(int argc, char* const argv[]) { \
         hal.run(argc, argv, CALLBACKS); \
         return 0; \
     } \
     }

        将这个宏替换到ArduCopter.cpp里的AP_HAL_MAIN_CALLBACKS(&copter)，它就变成了：

 int __EXPORT ArduPilot_main(int argc, char* const argv[]);
 int __EXPORT ArduPilot_main(int argc, char* const argv[]) {
         hal.run(argc, argv, &copter);
         return 0;
     }

        因此实际上这个工程的main函数就是ArduCopter.cpp里的ArduPilot_main函数。那么这里可能又牵扯到了一个问题，ArduPilot_main函数又是怎么调用的呢？如果像以前我们经常使用的单片机裸机系统，入口函数就是程序中函数名为main的函数，但是这个工程里边名字不叫main，而是ArduPilot_main，所以这个也不像裸机系统那样去运行ArduPilot_main那么简单。区别在于这是跑的Nuttx操作系统，这是一个类Unix的操作系统，它的初始化过程是由脚本去完成的。注意一个重要的词——脚本，如果你对Nuttx的启动过程不是很熟悉，可以查看我先前写的一些文章。而在这里需要注意两个脚本，一个是ardupilot/mk/PX4/ROMFS/init.d里的rcS，另一个是rc.APM，这个脚本在rcS里得到了调用，也就是说，rcS就是为Nuttx的启动文件。

        那么到底调用ArduPilot_main的地方在哪里呢？查看rc.APM的最低端：

 echo Starting ArduPilot $deviceA $deviceC $deviceD
 if ArduPilot -d $deviceA -d2 $deviceC -d3 $deviceD start
 then
     echo ArduPilot started OK
 else
     sh /etc/init.d/rc.error
 fi

        其中ArduPilot是一个内嵌的应用程序，由编译生成的builtin_commands.c可知，这个应用程序的入口地址就是ArduPilot_main。

 {"ArduPilot", SCHED_PRIORITY_DEFAULT, 4096, ArduPilot_main},
 {"px4flow", SCHED_PRIORITY_DEFAULT, CONFIG_PTHREAD_STACK_DEFAULT, px4flow_main}

        这样的命令有很多，在rcS里就开始调用的。至于这些内置的命令是怎么生成的，就要了解PX4Firmware的编译过程了。查看px4.targes.mk。

 PX4_MAKE = $(v)+ GIT_SUBMODULES_ARE_EVIL=1 ARDUPILOT_BUILD=1 $(MAKE) -C $(SKETCHBOOK) -f $(PX4_ROOT)/Makefile.make EXTRADEFINES="$(SKETCHFLAGS) $(WARNFLAGS) $(OPTFLAGS) "'$(EXTRAFLAGS)' APM_MODULE_DIR=$(SKETCHBOOK) SKETCHBOOK=$(SKETCHBOOK) CCACHE=$(CCACHE) PX4_ROOT=$(PX4_ROOT) NUTTX_SRC=$(NUTTX_SRC) MAXOPTIMIZATION="-Os" UAVCAN_DIR=$(UAVCAN_DIR)

4、arducopter.cpp

        分析ardupilot这套代码首先就是拿arducopter.cpp开刀，

        Loop函数的设计框架既要准确，又要高效。总体设计是这样的：

        其一用一个计时器定时触发测量；其二所有测量过程都靠中断推进；其三在main函数里不断检查测量是否完成，完成就进行解算。测量过程还是比较耗时间的，基于这样的设计，测量和解算可以同时进行，不会浪费CPU时间在（等待）测量上。而通过计时器触发测量，最大限度保证积分间隔的准确。

         整个控制过程主要就集中在arducopter.cpp里面，首先就是scheduler_tasks[]中是需要创建的task线程，如下图中，接收机的rc_loop、arm_motors_check等，还记得上篇博文中介绍的解锁和上锁的操作么，就是在arm_motors_check函数中实现的，以固定的频率去采集遥控器信号。

1）setup函数

        然后就是setup函数，在该函数中做的事情还是比较全面的，其中内部调用了一个比较重要的函数init_ardupilot()，该函数做了一系列的初始化，该初始化和上一篇博文介绍的不一样，上一篇主要是配置底层硬件的（如STM32、sensors），而此处的主要是飞行前的检测比那个初始化的晚；比如检测是否有USB连接、初始化UART、打印固件版本、初始化电源检测、初始化RSSI、注册mavlink服务、初始化log、初始化rc_in/out（内部含有电调校准）、初始化姿态/位置控制器、初始化飞行模式、初始化aux_switch、使能失控保护、配置dataflash最后打印"Ready to FLY "。接下来就几个比较重要的函数（上述标红）进行深入分析。

 void Copter::init_rc_in()
 {
     //rc_map 结合AC_RCMapper.h
     channel_roll     = RC_Channel::rc_channel(rcmap.roll()-1);
     channel_pitch    = RC_Channel::rc_channel(rcmap.pitch()-1);
     channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
     channel_yaw      = RC_Channel::rc_channel(rcmap.yaw()-1);
     // set rc channel ranges
     channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);//4500
     channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX);
     channel_yaw->set_angle(4500);
     channel_throttle->set_range(g.throttle_min, THR_MAX);//1000

     channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
     channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
     channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
     //set auxiliary servo ranges
     g.rc_5.set_range_in(0,1000);
     g.rc_6.set_range_in(0,1000);
     g.rc_7.set_range_in(0,1000);
     g.rc_8.set_range_in(0,1000);
     // set default dead zones
     default_dead_zones();
     // initialise throttle_zero flag
 ap.throttle_zero = true;//注意这个，rc_map好以后把油门配置为0
 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
 }

 // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
 void Copter::init_rc_out()
 {
     motors.set_update_rate(g.rc_speed);
     motors.set_frame_orientation(g.frame_orientation);//配置机体类型（+/x）
     motors.Init();                                              // motor initialisation
 #if FRAME_CONFIG != HELI_FRAME
     motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);//配置油门最大值和最小值    motors.set_hover_throttle(g.throttle_mid);
 #endif

     for(uint8_t i = 0; i < 5; i++) {
         delay(20);
         read_radio();
     }
     // we want the input to be scaled correctly
     channel_throttle->set_range_out(0,1000);
     // check if we should enter esc calibration mode
     esc_calibration_startup_check();//电调校准，进入以后首先判断是否有pre-arm，如果没有则直接退出校准。校准过飞机的都知道这个
     // enable output to motors
     pre_arm_rc_checks();
     if (ap.pre_arm_rc_check) {
         enable_motor_output();//内部会调用motors.output_min()函数sends minimum values out to the motors，待会介绍该函数
     }
     // refresh auxiliary channel to function map
     RC_Channel_aux::update_aux_servo_function();
     // setup correct scaling for ESCs like the UAVCAN PX4ESC which
     // take a proportion of speed.
     hal.rcout->set_esc_scaling(channel_throttle->radio_min, channel_throttle->radio_max);
 }

        简单介绍如何如何控制电机转动以及cork() and push()，并在此基础上测试了关于scheduler_tasks[] 中的任务的执行频率是否可以达到要求。测试方法：在scheduler_tasks[] 任务列表的throttle_task中添加一个累加标志位counterflag，每执行一次throttle_task任务就对齐加1，加到100时使电机转动，测试结果约为5S时电机转动，理论是50HZ的周期（不加执行时间是需要2S转动）再加上每次需要的执行时间以后还是比较理想的。

 // output_min - sends minimum values out to the motors
 void AP_MotorsMatrix::output_min()
 {
     int8_t i;
     // set limits flags
     limit.roll_pitch = true;
     limit.yaw = true;
     limit.throttle_lower = true;
     limit.throttle_upper = false;
     // fill the motor_out[] array for HIL use and send minimum value to each motor
 hal.rcout->cork();
 /*Delay subsequent calls to write() going to the underlying hardware in
 order to group related writes together. When all the needed writes are
 done, call push() to commit the changes.
 This method is optional: if the subclass doesn't implement it all calls
 to write() are synchronous.*/
 for( i=0; i
 // AP_MOTORS_MAX_NUM_MOTORS=8
         if( motor_enabled[i] ) {
             rc_write(i, _throttle_radio_min);
 //下面会解析rc_write(uint8_t chan, uint16_t pwm)
         }
     }
 hal.rcout->push();
 /*Push pending changes to the underlying hardware. All changes between a call to cork() and push() are pushed together in a single transaction.This method is optional: if the subclass doesn't implement it all calls to write() are synchronous.*/
 }
 //由上述可知在通过HAL层配置数据时使用cork() and push()函数包装需要单次传输的数据。该方法就是为了实现对四个电机同时配置，避免由for语句产生的延时，也是强调同步（synchronous）。
 如下解析rc_write(uint8_t chan, uint16_t pwm)
 void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
 {
     if (_motor_map_mask & (1U<
         // we have a mapped motor number for this channel
         chan = _motor_map[chan];// mapping to output channels
     }
     hal.rcout->write(chan, pwm);//通过HAL层直接输出配置电调
 }

        Setup()函数的最后一句是fast_loopTimer = AP_HAL::micros()，获取micros()通过层层封装最终就是上篇博文中介绍的hrt，该时间会在下面的loop函数中使用。

2）loop函数

        该函数比较重要，fast_loop和schedule_task都是封装在该函数中的，下面主要讲述fast_loop。

 // Main loop - 400hz
 void Copter::fast_loop()
 {
     // IMU DCM Algorithm 里面有个update函数，这个函数就是读取解算好的角度信息
     read_AHRS();
     // run low level rate controllers that only require IMU data
     attitude_control.rate_controller_run();
     // send outputs to the motors library
     motors_output();
     // Inertial Nav
     read_inertia();
     // check if ekf has reset target heading
     check_ekf_yaw_reset();
     // run the attitude controllers
     update_flight_mode();
     // update home from EKF if necessary
     update_home_from_EKF();
     // check if we've landed or crashed
     update_land_and_crash_detectors();
     // log sensor health
     if (should_log(MASK_LOG_ANY)) {
         Log_Sensor_Health();
     }
 }

        read_AHRS() 内部使用 ahrs.update 更新 AHRS 数据。

 // run a full DCM update round
 Void AP_AHRS_DCM::update(void)
 {
     // tell the IMU to grab some data
     _ins.update();//update gyro and accel values from backends具体实现过程详见源码
     // ask the IMU how much time this sensor reading represents
     delta_t = _ins.get_delta_time();
 // Integrate the DCM matrix using gyro inputs
 //使用陀螺仪数据更新DCM矩阵（方向余弦矩阵：direction-cosine-matrix ），使用刚刚测量出来的陀螺仪值、以及上个周期对陀螺仪的补偿值进行角度更新
     matrix_update(delta_t);
     // Normalize the DCM matrix 归一化处理
     normalize();
     // Perform drift correction
     drift_correction(delta_t);
     // paranoid check for bad values in the DCM matrix
     check_matrix();
     // Calculate pitch, roll, yaw for stabilization and navigation
     euler_angles();
     // update trig values including _cos_roll, cos_pitch
     update_trig();
 }

        到此为止，卡住了。不是因为不懂C++的缘故，而是因为理  论  知  识  太  欠  缺 了~~~~
        所以还是好好的准备理论知识吧，大把大把的论文要看。我是看Mahony的和Paul的，现在主要是Paul的《 Direction Cosine Matrix IMU: Theory》，讲的实在是太好了，搞无人机必看啊，这篇文章的最后给出了Mahony论文的下载地址。

六、高大上的理论

        关于上面的这个Normalize the DCM matrix（ 归一化处理）很有深度，值得了解一下其原理，在使用DCM控制和导航时，肯定存在积累数值的舍入误差、陀螺漂移误差、偏移误差、增益误差。它主要就是补偿抵消这几种误差（注意这几种误差 error）的；使用PI负反馈控制器检测这些误差，并在下一次产生之前就抵消这些误差（GPS检测yaw error，加速度计检测pitch和roll error）。在ardupilot源码中使用的Normalize算法就是来自2009年Paul的研究成果--Direction Cosine Matrix IMU: Theory。首先了解一下几个比较关键的概念。用下图先有个感性认识吧

1、陀螺漂移（Gyro drift）

        由于外干扰力矩（机械摩擦、振动等因素）引起的陀螺自转轴的缓慢进动：陀螺漂移。通常，干扰力矩分为两类，与之对应的陀螺漂移也分为两类：一类干扰力矩是系统性的，规律已知，它引起规律性漂移，因而是可以通过计算机加以补偿的；另一类是随机因素造成的，它引起随机漂移。在实际应用中，除了要尽可能减小随机因素的影响外，对实验结果还要进行统计处理，以期对随机漂移作出标定，并通过系统来进行补偿。但由于它是无规律的，很难达到。

        平时所说的用加速计（静态特性好）修正陀螺仪的漂移，其实这个修正是利用加速度计修正陀螺仪计（动态特性好）算出的姿态，并估计出陀螺仪的漂移，在下一次做姿态解算时，陀螺仪的输出减去估计出的漂移后再做捷联姿态解算，以此不断循环。融合的方法一般用EKF，KF也是基于最优估计的。

2、误差来源

        在进行数值积分的过程中一定会引入数值误差，数值误差分为两类；一类是积分误差（来自于我们假设旋转速率在短时间内不变引起的），另一类是量化误差（主要来自于模数转换过程中引起的）。关于gyro误差产生的详细分析见：http://www.crazepony.com/wiki/mpu6050.html

3、旋转矩阵的约束

        旋转矩阵就是改变方向不改变大小；旋转矩阵有9个元素，实际上是只有三个是独立的。由于旋转矩阵的正交性，意味着任何一对行或者列都是相互垂直的，并且每个行或者列的元素的平方和为1。所以在9个元素中有6个限制条件。

4、误差导致的结果

        旋转矩阵的关键性能之一就是正交性，即在某个参考坐标系下的三个方向的向量两两垂直，且向量长度在每个参考系下都是等长的。数值误差会违背该性能，比如旋转矩阵的行和列都是单位向量，其长度都是1，但是由于数值误差的原因导致其长度边长或变短；最终致使它们缩小到0或者增长到无穷大，最后的结果就是导致原本相互正交的现在变的倾斜了。如下图所示。

        那么问题来了，如何修正这个问题呢？可以使用如下方法（  Ardupilot源码中就是利用如下算法处理的errors）。

5、如何消除各种误差（积累数值的舍入误差、陀螺漂移误差、偏移误差、增益误差）

        其方法就是采样DCMIMU:Theory中提出的理论—强制正交化（也称为Renormalization）。

        首先计算矩阵中X、Y行的点积（dotproduct），理论上应该是0，但是由于各种errors的影响，所以点积的值不是0，而表示为error。

        然后把这个误差项分半交叉耦合到X、Y行上，如下公式。

        通过上述两个公式处理过以后，正交误差明显减小了很多，即R旋转矩阵的每个行和列的长度都非常接近1。 接下来就是调整旋转矩阵的Z行，使其与X、Y正交，方法比较简单，直接使用X、Y的叉积（cross product）即可。

        最后一步就是放缩旋转矩阵的每一行使其长度等于1，现在用一种比较简单的方法实现，即使用泰勒展开。

        ardupilot中的源码实现如下：

 Void AP_AHRS_DCM::normalize(void)
 {
     float error;
     Vector3f t0, t1, t2;
     error = _dcm_matrix.a * _dcm_matrix.b;     // eq.18
     t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error));      // eq.19
     t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error));      //eq.19
     t2 = t0 % t1;                            // c= a x b // eq.20
     if (!renorm(t0, _dcm_matrix.a) ||
             !renorm(t1, _dcm_matrix.b) ||
             !renorm(t2, _dcm_matrix.c)) {
         // Our solution is blowing up and we will force back
         // to last euler angles
         _last_failure_ms = AP_HAL::millis();
         AP_AHRS_DCM::reset(true);
     }
 }

七、结论
        本篇博文没有介绍多少关于源代码的，主要就时觉得理论太欠缺了，接下来还有四元数，控制中主要还是用它做运算，还有各种滤波。接下来就是主要看关于姿态估计的了， 姿态估计算法实现主要就是在PX4Firmware/src/modules/attitude_estimator_q中（q：quaternion），首先介绍一些代码执行顺序，方便后期有个良好的逻辑框架阅读代码和习惯。

        1）  首先就是该文件中需要的头文件的包含；

        2）  然后是一个C++的引用定义extern"C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])，可以根据这个attitude_estimator_q_main进行追踪到函数原型（754）；

        3）  在attitude_estimator_q_main函数中调用姿态估计的启动函数start()（775）；

        4）  详细介绍一下该启动函数，比较重要，源码中很多都是靠这种方法启动的，还记上次讲的sensor初始化么。源码如下。

 int AttitudeEstimatorQ::start()
 {
     ASSERT(_control_task == -1);
     /* start the task *//*POSIX接口的任务启动函数*/
     _control_task = px4_task_spawn_cmd("attitude_estimator_q",
                        SCHED_DEFAULT,
                        SCHED_PRIORITY_MAX - 5,
                        2100,
  (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
                        nullptr);
     if (_control_task < 0) {
         warn("task start failed");
         return -errno;
     }
     return OK;
 }

        5）  然后是task_main_trampoline函数，内部跳转到 task_main()

        好了，就是它了，慢慢研究吧，把这个里面的过程都研究透吧~~~~至少好几天