PX4利用matlab推导生成 EKF中雅可比矩阵的方法

PX4实际使用了Matlab的Symbolic Maths Toolbox来进行雅可比矩阵的推导，看完matlab脚本之后，感觉打开了新世界的大门，确实是相见恨晚。Matlab还有多少惊喜是我不知道的。可见这个世界上有很多知识都已经存在，只是由于自己孤陋寡闻，所以一直在用笨方法——手动推导。由此也可知，多看看优秀的人是怎么做的，站在巨人的肩膀上，让我们可以少走很多弯路。

一、Matlab的Symbolic Maths Toolbox是什么

Symbolic Math Toolbox支持以解析方式执行微分、积分、化简、变换和方程求解。具体可参见官方文档，下面列出了几个主要功能。

微积分
计算定积分和不定积分的精确解析解，计算符号表达式或函数的导数，并使用级数展开式逼近函数。

求解、化简和代换
使用解析法求解线性和非线性代数方程与微分方程，化简并重写符号表达式，同时使用代换法计算符号表达式。

线性代数
对符号矩阵进行分析、变换和分解，研究线性方程的属性，进行线性代数运算，并求解矩阵或方程形式的线性方程组。

代码生成
直接从符号表达式生成 MATLAB 函数、Simulink 函数模块、基于方程的自定义 Simscape 组件以及 C 或 Fortran 代码。

二、PX4 EKF中需要的雅可比矩阵

参考博客PX4 的 ECL EKF 公式推导及代码解析，在讨论状态向量的协方差传播时，我们首先需要知道状态量的转移矩阵，如下式。我们根据第 k+1 时刻的状态向量方程，对第k时刻的状态量求Jacobian得到状态转移矩阵 F 和 G。

然后得到状态误差矩阵：

则协方差的传播公式为：

在进行各种传感器数据融合时，我们需要得到传感器观测值对状态量x的观测矩阵H，这里也需要求Jacobian矩阵。如对于GPS的位置、速度融合来说，量测矩阵如下。位置、速度融合的量测矩阵还是比较直观的。如果是航向融合，观测矩阵就会复杂得多，毕竟航向和四元数状态量之间关系就不是那么直接了，如果手动推导就比较费功夫。

三、 Matlab脚本解析

PX4 推导EKF中的雅可比矩阵的matlab脚本为GenerateEquations24.m，该脚本可以生成EKF中的状态转移矩阵F，状态误差矩阵Q，以及各传感器融合的观测矩阵H。

（1）脚本的1~18行主要是定义一堆符号变量，包括状态量等。 syms定义符号变量默认是复数，添加“real”可以使一个变量变为实数。

（2）脚本的第19~85主要是定义状态预测（传播）方程


（3）脚本第86~105行：生成计算状态转移矩阵F和状态误差矩阵Q的matlab函数，写入calcF24.m和calcQ24.m

(4)脚本第106~121行：生成计算三轴磁融合的观测矩阵的matlab函数

剩下的脚本也都是计算各种Jacobian矩阵，不再一一列举。

由于这个脚本中没有GPS航向融合的Jacobian计算，因此我自己也尝试写了一段matlab代码，用来生成计算航向融合时观测矩阵的matlab函数，该函数计算结果与C代码一致。我自己也进行了手动推导，计算公式与matlab生成的函数一致，因此也验证matlab生成的函数是正确的，但是显然比手动推导省时省力。

参考来源

[1] 推荐一个相见恨晚的Matlab Toolbox - Symbolic Maths（数学难度-50%）
[2] PX4 的 ECL EKF 公式推导及代码解析