文章目录
PX4中的EKF模型
px4中ekf代码中有大量自动生成的代码（矩阵部分），其c代码由matlab生成，生成脚本在ecl中，见generateNavFilterEquations.m
sympy
python是免费的，使用python也可以进行上述工作，当然似乎sympy这方面还是没有matlab做的强大，例如OptimiseAlgebra.m这个文件会把推导的矩阵中的相同子表达式提取出来，提高计算效率。这个功能暂时用sympy还不好做，也可能是我还没找到。
python实例
generateNavFilterEquations.m中的模型为24维模型，在这里把python版本写出来有点大，此处只是举个例子，因此只写一下四元数部分，以下是python代码。
from sympy import *
import sympy as sym
# define symbolic variables and constants
# IMU delta angle measurements in body axes - rad
dax = sym.Symbol('dax')
day = sym.Symbol('day')
daz = sym.Symbol('daz')
# quaternions defining attitude of body axes relative to local NED
q0 = sym.Symbol('q0')
q1 = sym.Symbol('q1')
q2 = sym.Symbol('q2')
q3 = sym.Symbol('q3')
# delta angle bias - rad
dax_b = sym.Symbol('dax_b')
day_b = sym.Symbol('day_b')
daz_b = sym.Symbol('daz_b')
# IMU time step - sec
dt = sym.Symbol('dt')
dAngMeas = sym.Matrix([dax, day, daz])
# define the IMU bias errors and scale factor
dAngBias = sym.Matrix([dax_b, day_b, daz_b])
# define the quaternion rotation vector for the state estimate
quat = sym.Matrix([q0,q1,q2,q3])
dAngTruth = dAngMeas - dAngBias
# define the attitude update equations
# use a first order expansion of rotation to calculate the quaternion increment
# acceptable for propagation of covariances
deltaQuat = sym.Matrix([1,0.5*dAngTruth[0], 0.5*dAngTruth[1],0.5*dAngTruth[2]])
quatNew_0 = quat[0]*deltaQuat[0]-quat[1]*deltaQuat[1]-quat[2]*deltaQuat[2]-quat[3]*deltaQuat[3]
quatNew_1 = quat[0]*deltaQuat[1] + deltaQuat[0]*quat[1]+quat[2]*deltaQuat[3]-quat[3]*deltaQuat[2]
quatNew_2 = quat[0]*deltaQuat[2] + deltaQuat[0]*quat[2]+quat[3]*deltaQuat[1]-quat[1]*deltaQuat[3]
quatNew_3 = quat[0]*deltaQuat[3] + deltaQuat[0]*quat[3]+quat[1]*deltaQuat[2]-quat[2]*deltaQuat[1]
quatNew = sym.Matrix([quatNew_0,quatNew_1,quatNew_2,quatNew_3])
# define the IMU error update equations
dAngBiasNew = dAngBias;
stateVector = sym.Matrix([quat[0],quat[1],quat[2],quat[3],
dAngBias[0],dAngBias[1],dAngBias[2]])
# Define vector of process equations
newStateVector = sym.Matrix([[quatNew[0],quatNew[1],quatNew[2],quatNew[3],
dAngBiasNew[0],dAngBiasNew[1],dAngBiasNew[2]]]).T
F = newStateVector.jacobian(stateVector)
输出F矩阵如下，
Matrix([
[ 1, -0.5*dax + 0.5*dax_b, -0.5*day + 0.5*day_b, -0.5*daz + 0.5*daz_b, 0.5*q1, 0.5*q2, 0.5*q3],
[0.5*dax - 0.5*dax_b, 1, 0.5*daz - 0.5*daz_b, -0.5*day + 0.5*day_b, -0.5*q0, 0.5*q3, -0.5*q2],
[0.5*day - 0.5*day_b, -0.5*daz + 0.5*daz_b, 1, 0.5*dax - 0.5*dax_b, -0.5*q3, -0.5*q0, 0.5*q1],
[0.5*daz - 0.5*daz_b, 0.5*day - 0.5*day_b, -0.5*dax + 0.5*dax_b, 1, 0.5*q2, -0.5*q1, -0.5*q0],
[ 0, 0, 0, 0, 1, 0, 0],
[ 0, 0, 0, 0, 0, 1, 0],
[ 0, 0, 0, 0, 0, 0, 1]])
另外，使用print(latex(F))还可以打印出latex公式，如下：