链接：http://www.zhihu.com/question/38874663/answer/84239995
来源：知乎
以下所描述的都是针对px4原生固件，此外，由于固件更新过于频繁，本文描述的是15年7月的固件，主要是举例，有改动的话，自己再研究研究吧（后面换cmake编译方式了，改动蛮大）。
既然要做开发，第一步就是搭好开发环境，根据我的经验，最好是在linux环境下编译，这样效率会很快，以前在windows下编译，经常40分钟以上，这样就太影响开发了；
第二步，大概了解下固件的架构，
如果只涉及应用层的开发，那底层的nuttx系统就可以绕过去了，一般，最好先把uorb模块的机制整明白就好了，从uorb入手，了解每个话题的来源以及作用，整理数据流，清楚每个模块之间的关系即可，比如，要实现手动模式，哪些模块互相交互，auto模式，又有哪些模块起作用，
如果涉及相应算法的开发，要学会定位到相应的算法模块，甚至具体到哪些代码，比如，你想试验你的姿态估计算法，那你就将姿态估计模块替换掉即可，不过相应的接口仍需要和px4环境一样，以姿态估计为例，最后要发布你的vehicle_attitude话题，不然无法与其他模块交互；
另外，不要试图在代码中找main函数，那是单片机思维，你只需看启动脚本即可，\ROMFS\px4fmu_common\init.d\rcs；
第三步，针对你的具体情况，定位相应的模块，进行精读研究，虽然模块基本是用C++写的，但是不会C++也没关系，毕竟又不是让你写，本人倒目前为止，也不会C++，配合注释，看明白就好了，比如，整理下mavlink的控制流程；
px4原生固件模块列表：
系统命令程序
mavlink –通过串口发送和接收mavlink信息
sdlog2 –保存系统日志/飞行数据到SD卡
tests –测试系统中的测试程序
top –列出当前的进程和CPU负载
uORB – 微对象请求代理器-分发其他应用程序之间的信息
驱动
mkblctrl–blctrl电子模块驱动
esc_calib –ESC的校准工具
fmu –FMU引脚输入输出定义
gpio_led –GPIOLED驱动
gps –GPS接收器驱动
pwm –PWM的更新速率命令
sensors –传感器应用
px4io –px4io驱动
uavcan –uavcan驱动
飞行控制的程序
飞行安全和导航
commander –主要飞行安全状态机
navigator –任务，失效保护和RTL导航仪
估计姿态和位置
attitude_estimator_ekf –基于EKF的姿态估计
ekf_att_pos_estimator –基于EKF的姿态和位置估计
position_estimator_inav–惯性导航的位置估计
multirotor姿态和位置控制器
mc_att_control–multirotor姿态控制器
mc_pos_control –multirotor位置控制器
fixedwing姿态和位置控制器
fw_att_control –固定翼飞机的姿态控制
fw_pos_control_l1 –固定翼位置控制器
垂直起降姿态控制器
vtol_att_control –垂直起降姿态控制器
最后提一句，多看看官网的说明，另外根据本人的经验来看，由于大框架，代码人家都写好了，通常你要加功能，所修改的也就几行代码而已，举例说明，比如px4固件只能在手动模式解锁，假如我要修改成定高模式解锁
希望可以帮助你，欢迎采纳
本文为离散卡尔曼滤波算法的一 一个简明教程，从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。
XU Ruilin完成本教程主要部分的编写，WANG Xuejun完成第3节的编写，ZHU Ximin完成2.2节的编写，WEN Shuhan完成2.3节的编写，MAO Bo完成全文整理、修订和排版。
卡尔曼滤波（Kalman Filtering）及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。 鲁道夫·卡尔曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心访问时，发现他的方法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。
卡尔曼滤波尤其适用于动态系统，这种方法对于内存要求极低而运算速度快，且能够保持较好的计算精度，这使得这种方法非常适合解决实时问题和应用于嵌入式系统，也就是说，卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里，我们将逐步领会卡尔曼滤波的这些绝佳特点。
不过，现在我们先从复杂的舰艇航迹推算问题中解脱出来，从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。
假设有一辆无人车WALL-E，需要导引它从A点到达B点，共有两种手段（ 图1 ）：
显然，两种方法都有一定的误差。如果单独采用某一种方法进行定位，WALL-E在误差的影响下将无法到达B点。因此，需要将两种方法结合起来，得到一个更加精确的结果，这就是卡尔曼滤波要解决的问题。
卡尔曼滤波方法如何看待我们的问题呢？在探究这个问题之前，我们先对问题进行抽象，并用数学语言来描述我们的问题。
我们用矢量 来描述WALL-E的运动状态，这个列矢量 包括位置矢量 和速度矢量 两个分量。在WALL-E的问题上，我们现在不知道位置 和速度 的准确值，但是知道WALL-E的运动模型满足 状态方程 ，定位的方法，也即观测WALL-E运动状态的方法满足 观测方程 . 当然，我们也知道，这两种方法都存在一定的误差 ，那么我们的问题就可以转化为一个优化问题——
在这一优化问题中，目标函数是要使预测（估计）误差最小，同时约束于估计方法 和 的条件下。在卡尔曼滤波中，我们的估计原则（也就是最小化估计误差的原则）是 最小方差无偏估计 [1] ，我们将通过后面的过程分析来说明这一点。
在我们正式开始引入公式分析卡尔曼滤波问题之前，我们还必须解决一个问题------把连续的线性系统离散化，也就是将连续时域问题转化为时间序列问题。当然，目前我们只讨论线性系统的情况，关于非线性系统问题，我们有扩展卡尔曼滤波（Extended Kalman Filtering, EKF）和无迹卡尔曼滤波（Unscented Kalman Filtering, UKF）两种方法来求解。
补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为
若采样周期为 ，则从时刻 到时刻 ，有
令 ， ，则离散化的状态方程为
通过对线性系统的离散化处理，我们现在可以考虑每一个时刻WALL-E的运动状态。接下来，我们将用 来表示在 时刻运动状态的最优估计值；用 表示用 时刻对 时刻的状态预测值；用 表示对 时刻综合预测和观测两种方法的最优估计值。
在估计WALL-E位置的问题上，假定我们已经知道它是匀速直线运动，WALL-E身上还携带有一个GPS传感器可以提供它的位置信息，WALL-E在前进过程中可能会遇到一些情况，比如停止前进或是受到风的影响。
加入我们已知的是WALL-E上一个时刻的最佳估计状态，即k-1时刻的位置和速度，要求的是下一时刻即k时刻的最佳估计状态，即k时刻的位置和速度，我们可以发现有两种方法可以得到它的k时刻的状态：
一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。
另一种是通过WALL-E所携带的GPS来确定它的位置，因为GPS是测量出的就是WALL-E的实时状态，因此它比较准确。但是GPS测量k时刻的状态有两个问题，一是GPS只能测出WALL-E的位置，而测不出它的速度；二是GPS传感器测量的时候也会有仪器的误差，只能说它是比较准确的，比较接近真实值的。
那么接下来问题来了，我们如何得到k时刻WALL-E的真实状态呢？
我们将第一种方法得到的状态值称为预测值，第二种方法得到的状态值称为测量值，对汽车的最佳估计就是将这两部分信息结合起来，尽量的去逼近k时刻的真实值。
下面再深入一些思考，怎么将这两部分结合起来？
在初始时间k-1， 是WALL-E的最佳估计值，WALL-E其实可以是估计值附近的任何位置，并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间，估计的不确定性增加，用一个更大的方差表示，这是因为在时间步骤k-1和k之间，WALL-E可能收到了风力的影响，或者脚可能向前滑了一点，因此，它可能已经行进了与模型预测的距离不同的距离。
WALL-E位置的另一个信息来源来自测量，方差表示误差测量的不确定性，真正的位置同样可以是平均值附近的任何位置。
预测值和测量值，对WALL-E的最佳估计是将这两部分信息结合起来，将两个概率函数相乘得到另一个高斯函数，该估计值的方差小于先前估计值，并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。
以下，我们将进行e的运算推导
设：
则有实际目标变量的表达式：
数学模型中目标变量的表达式：
实际模型中测量变量的表达式：
数学模型中测量变量的表达式：
将目标变量的实际值和估计值相减：
将上述方程带入误差e的表达式，我们可得出误差e的解析解：
从推导结果中我们不难看出，估计值和实际值的误差随时间呈指数形式变化，当（F-KH）<1时，随着时间的推移，会无限趋近于零，也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。
在估计WALL-E位置的问题上，我们不知道位置 和速度 的准确值，但是我们可以给出一个估计区间（ 图5.a ）。卡尔曼滤波假设所有的变量是随机的且符合高斯分布（正态分布）。每个变量有一个均值 和一个方差（ 图5.b ）。而 图5.c 则表示速度和位置是相关的。
假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，WALL-E不会走的很远。这种关系在跟踪系统状态时很重要，它给了我们更多的信息：一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。
根据数理统计知识，我们知道这种两个观测值（随机变量）之间的关系可以通过一个协方差矩阵
描述（ 图6 ）。
我们假设系统状态的分布为 高斯分布（正态分布） ，所以在 时刻我们需要两个信息：最佳预估值 及其协方差矩阵 （如式(2)所示）。
下一步，我们需要通过 时刻的状态来预测 时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎，它仅仅是对 时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。在这个过程中，位置 和速度 的变化为
我们可以通过一个状态转移矩阵 来描述这个转换关系
同理，我们更新协方差矩阵 为
到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为
引入矩阵表示为
式中 称为控制矩阵， 称为控制向量（例如加速度 ）。当然，如果没有任何外界动力影响的系统，可以忽略这一部分。
我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？如果状态只会根据系统自身特性演变，那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确，那样也不会有任何问题。但是如果有些外力我们无法预测，例如我们在跟踪一个四轴飞行器，它会受到风力影响；或者在跟踪一个轮式机器人，轮子可能会打滑，地面上的突起会使它减速。我们无法跟踪这些因素，而这些不确定事件发生时，预测方程将会失灵。因此，我们将这些不确定性统一建模，在预测方程中增加一个不确定项。
通过这种方式，使得原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点（ 图7.a ）。 可以这样描述------ 中的每个点移动到一个符合方差 的高斯分布里（ 图7.b ）。换言之，我们把这些不确定因素描述为方差为 的高斯噪声，并用 表示。这样就会产生一个新的高斯分布，方差不同，但是均值相同（ 图7.c ）。
通过对 的叠加扩展，得到完整的预测转换方程为
新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里，我们得到了一个模糊的估计范围------通过 和 描述的范围。
我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态，现在，我们要把另一种方法，也就是观测（本问题中为GPS定位）考虑进来，以进一步修正对运动状态的估计（ 图8 ）。
我们用矩阵 来描述观测方法的作用，于是有
再加入观测噪声 ，观测方程为
从控制论的角度出发，我们定义新息（也即观测值与预测值的误差）为
当然我们也知道，观测本身也会存在误差，比如本问题中的GPS定位精度仅有10m. 因此，我们用矩阵 来描述这种不确定性（ 图10 及 图11.a ）。
这时，我们新息的协方差为
现在我们需要把两种方法得到的可能性融合起来（ 图11.b ）。对于任何状态，有两个可能性：1. 传感器的观测值更接近系统真实状态；2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式，并且我们想知道两者都准确的概率值，于是我们可以通过加权来解决更相信谁的问题（ 图11.c ）。
我们现在知道，系统模型的状态预测 与对系统的状态观测 服从高斯分布，把这个问题抽象一下就是——
根据我们的一个估计准则------ 最小方差估计 ，那么这个问题可以转化为优化问题求解
求导数（差分）得
则 ，从而
当维度高于一维时，我们用矩阵来描述，有
这里的 称为 卡尔曼增益 （Kalman Gain），也就是我们在解决更信任哪种方法时的偏向程度。
如果我们从两个独立的维度估计系统状态，那么根据系统模型的预测为
通过传感器的观测为
我们结合着两种方法得到
由 可知，卡尔曼增益为
将 约去（ 中也含有 项），得
此时的卡尔曼增益实际为
我们最后再来验证一下 估计的无偏性 ——
这里我们设 时刻的真值为 ，由于
由于 （ 从初值而来的无偏传递性 ）可知 ，即卡尔曼滤波满足无偏估计准则。显然，其中要求系统噪声和观测噪声是不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，卡尔曼滤波的估计结果是有偏的。
到这里，我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设
假设一 和 都是零均值高斯白噪声，也即 ，
假设二 与 无关，也即
假设三 系统初值 的均值和方差已知，且 与 均不相关。
以及5个基本方程 方程一 状态预测
方程二 协方差预测
方程三 卡尔曼增益
其实理论上来说地磁罗盘错误并不会导致坠机，只会导致GPS悬停或者waypoint following功能失效，飞机会往错误的方向越飞越远而已。
从硬件放置的角度上说，ESC，电池，应该尽可能远离飞控（内置罗盘）和GPS（外置罗盘）。在第一次飞行前应该进行一次地面测试，缓慢地将油门打到100%，看看罗盘指向或者磁场干扰是否稳定和在合理范围内。否则不建议直接飞行，至少不要用自动模式飞行（悬停loiter，waypoint following，回家RTL等等）。
从飞控软件的角度说，如果飞控侦测到罗盘干扰很大，或者EKF里磁力罗盘变化率和gyro，gps的外置罗盘的观测值出现了较大的差别时，飞控可以短时间内只通过gyro来估测方向，同时蜂鸣器发出警告 （ardupilot在这个时候会闪黄灯而不是正常的蓝灯）。如果 *** 控者发现这一情况，应该立刻 *** 控飞机往空旷的区域飞以远离干扰源。一般情况下单单靠gyro来估算方向只能撑最多十几秒，所以十几秒后如果干扰还是很大，飞控只能回到自稳模式让 *** 控者自行控制，或者缓慢降落。但其实只要没有因为方向错乱而撞到什么，应该不至于坠机。
你要说不依赖地磁罗盘导航的，学校倒是有几个视觉追踪汽车、运动员的飞机，飞控不需要理会经纬坐标系而只使用body frame坐标系即可实现自动飞行的飞机。
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）