PX4代码学习系列博客(7)——lpe代码分析

本文主要内容：

本文主要介绍px4飞控的位置估计中的lpe。在笔者原先的博客中曾经写过可以把px4中的位置估计算法移植到桌面电脑，当时用的开发环境是vs。但是那篇文章并没有对lpe位置估计的数学原理和程序的具体执行流程做出解释，那这部分工作就在本文中来做。

lpe的目录下各文件介绍

在local_position_estimator文件夹下：
sensors文件夹下是估计用到的气压，光流，gps，mocap，超声波，视觉等的初始化和卡尔曼滤波的修正函数，这些函数在BlockLocalPositionEstimator.cpp中被调用。

BlockLocalPositionEstimator.cpp和BlockLocalPositionEstimator.h是BlockLocalPositionEstimator这个类的定义，这个类就是lpe的核心，后面专门用一节讲这个类。

local_position_estimator_main.cpp，这个文件是nuttx操作系统的一个应用程序，它启动local_position_estimator_thread_main这个任务，这个任务函数的代码比较短，可以直接粘贴过来

int local_position_estimator_thread_main(int argc, char *argv[]){    PX4_DEBUG("starting");    using namespace control;    BlockLocalPositionEstimator est;       //定义一个lpe估计类的实例    thread_running = true;    while (!thread_should_exit)     {        est.update();                  //调用这个类的update方法进行卡尔曼估计    }    PX4_DEBUG("exiting.");    thread_running = false;    return 0;}

估计位置用到的各个模块分析

在lpe中，用到的模块有气压，光流，gps，mocap，超声波，视觉，这些cpp文件的写法风格和函数名称都比较类似，这里选择其中一个mocap进行介绍，因为mocap中三维的消息都有，其他超声波只有在高度，光流只有平面位置，gps只有在室外才能用，视觉的门槛高，精度又不高。

在mocap中只有4个函数（其他模块也一样）

mocapInit：在第一次连续接受到20帧mocap消息就任务初始化完毕，这个函数在上电后第一次使用或者mocap消息超时以后会被调用。mocap消息超时指连续200ms没有接收到mocap消息，这时px4认为mocap消息断开了，会重新调用初始化函数重新开始。在实际测试过程中，由于通信的原因，偶尔的两帧消息之间超过200ms导致的超时不会产生很大的影响，一般在1s之内它能够重新初始化，或者把源码中的REQ_MOCAP_INIT_COUNT改小也可以使偶尔因为通信带来的超时造成的影响变小。

mocapMeasure：这函数把订阅到的mocap消息赋值到卡尔曼的测量矩阵，并记录收到mocap消息的时间。

mocapCorrect：卡尔曼修正函数，里面是数学矩阵的运算，具体可以查阅卡尔曼滤波相关内容。

mocapCheckTimeout：检查是否超时。

BlockLocalPositionEstimator类

这个类中以publish开头的函数是把估计出的位置通过uORB发送出去。

在讲下面两个函数前先简单说说卡尔曼的函数实现，其实就4个字，“预测”，“修正”，lpe用加速度计信息和飞机当前姿态进行位置的预测，用sensors文件夹的各个模块进行修正。

predict函数：这个函数利用当前的加速度计信息和当前飞机的姿态进行位置的预测。使用了runge kutta methods 的卡尔曼的公式，数学推导没看太懂，从代码上理解的话就好像本来是2，现在写成1+1，可以看看它的代码：

Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(    float t,    const Vector<float, BlockLocalPositionEstimator::n_x> &x,    const Vector<float, BlockLocalPositionEstimator::n_u> &u){    return _A * x + _B * u;}float h = getDt();Vector<float, n_x> k1, k2, k3, k4;k1 = dynamics(0, _x, _u);k2 = dynamics(h / 2, _x + k1 * h / 2, _u);k3 = dynamics(h / 2, _x + k2 * h / 2, _u);k4 = dynamics(h, _x + k3 * h, _u);Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);

update函数中调用了上一节中各个模块的修正函数，比如mocapCorrect，当然在调用前有一些判断机制，看mocap消息是否有更新，是否启用mocap，是否超时等。还调用了预测函数predict。

其他的一些函数是对卡尔曼滤波矩阵的赋值，有些相当于滤波参数，可以直接在地面站进行修改。这里用的卡尔曼滤波的A矩阵大小是10*10，有时间可以在纸上推演一遍重新写一篇博客，专门对这里面的用到是卡尔曼滤波的数学过程进行推导。