第十一章 PX4-Pixhawk-LPE位置估计



 位置估计的解析我们还是遵循源代码的规则，至于组合惯导和扩展卡尔曼我们以后慢慢分析，这里主要还是对源代码来进行解析。

 在前一章节我们找到SYS_MC_EST_GROUP参数设置的是1，采用的是LPE算法，所以也就启动的是local_position_estimator.cpp。回到该cpp文件中，我们开始一步一步的进行解析。

 还是一样找到入口函数“local_position_estimator_main”函数，找到启动函数“local_position_estimator_thread_main”线程。接着完成类的定义和构造，然后就能找到相应启动的时“void BlockLocalPositionEstimator::update()”函数，注意这个是个循环函数。采用了poll函数来确定函数运行的周期。首先计算出程序跟新周期检测飞机状态是否解锁，如果没有解锁就读取高度测量的值这里主要是雷达数据和超声波数据的更新，这里不做细讲，有兴趣的可以自己看一下。然后开始订阅各个数据，包括光流数据、gps数据、传感器数据等等。updateSSParams函数更新R矩阵和Q矩阵。接着判断X坐标和Y坐标数据的有效性，然后再判断Z轴有效性，还有就是地形的数据有效性。完了之后就是获取时间。

  所有数据方面的工作做完后就开始预测估计了这个函数就是“predict();”这里通过获取的四元素经过矩阵运算得到旋转矩阵并转化为地理坐标系，接着利用方向余弦矩阵转化得到A矩阵updateSSStates()。

 现在需要求出位置的增量，这里利用的是四姐龙格塔库算法求解，至于原理是什么可以自己到给出的网址上去看原理https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods，数据经过滤波后进行存储。

  回到函数中开始更新GPS数据，一个初始化完成后执行，一个初始化执行。这里不对初始化来解析，自己去看。GPS拿到gps的经纬和高还有NED方向的速度，这个函数map_projection_project涉及到在地球上通过经纬来确定本身的移动的变化，这个理论网上有很多，可以去看看。计算出来后得到C矩阵，接下来的本人也有点模糊，需要自己去看看。

  再回到函数一样的分析，气压计的数据的更新，雷达数据更新，超声波数据更新、光流的更新，相同的分析方法这里就不讲了。完了之后当然是发布数了，，这里自己慢慢看吧，本人对LPE这块不熟，对于这块的飞行怎么样也没试过，有兴趣的可以试一下，然后评论一下告诉我。