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

一、开篇

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

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

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

三、实验平台

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下非常easy出现故障，所以建议还是在控制台编译吧，并且有非常炫的颜色。

假设是拷贝别人的源代码，可能会出现无法编译的情况，原因不是文件丢失的问题，而是编译一次以后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;
    }

因此实际上这个project的main函数就是ArduCopter.cpp里的ArduPilot_main函数。那么这里可能又牵扯到了一个问题，ArduPilot_main函数又是怎么调用的呢？假设像曾经我们常常使用的单片机裸机系统，入口函数就是程序中函数名为main的函数，可是这个project里边名字不叫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; 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<<chan)) {
        // 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或者增长到无穷大。最后的结果就是导致原本相互正交的如今变的倾斜了。

例如以下图所看到的。

watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQv/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/Center" align="middle" alt="">

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

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

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

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

watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQv/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/Center" align="middle" alt="">

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

watermark/2/text/aHR0cDovL2Jsb2cuY3Nkbi5uZXQv/font/5a6L5L2T/fontsize/400/fill/I0JBQkFCMA==/dissolve/70/gravity/Center" align="middle" alt="">

通过上述两个公式处理过以后。正交误差明显减小了非常多。即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()

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