PX4的无人机飞控应用开发

1、PX4/PixHawk无人机飞控应用开发1、PX4/Pixhawk飞控软件架构简介PX4是目前最流行的开源飞控板之一。PX4的软件系统实际上就是一个firmware，其核心OS为NuttX实时ARM系统。其固件同时附带了一系列工具集、系统驱动/模块与外围软件接口层，所有这些软件（包括用户自定义的飞控软件）随OS内核一起，统一编译为固件形式，然后上传到飞控板中，从而实现对飞控板的软件配置。PX4配套的软件架构主要分为4层。理解其软件架构是开发用户自定义飞控应用软件的基础。a) API层：这个好理解。b) 框架层：包含了操作基础飞行控制的默认程序集（节点）c) 系统库：包含了所有的系统库和基本交通

2、控制的函数d) OS内核：提供硬件驱动程序、网络、UAVCAN和故障安全系统上述是个面向PX4系统实现者的相对具体的软件架构。实际上还有另外一种面向PX4自定义飞控应用开发者的高层软件架构描述，相对抽象，但更简单，就是整个PX4的软件从整体上分为2层：a) PX4 flight stack：一系列自治无人机自动控制算法的集合b) PX4 Middleware：一系列针对无人机控制器、传感器等物理设备的驱动及底层通信、调度等机制的集合PX4软件架构中，最有意思的一点在于整个架构的抽象性（多态性）。即，为了最大限度保障飞控算法代码的重用性，其将飞控逻辑与具体的底层控制器指令实现进行了解耦合。一套高

3、层飞控算法（如autopilot、GeoFence等）在不做显著修改的情况下，能够适用于固定翼、直升机、多旋翼等多种机型的控制场合，这时候就体现出PX4飞控的威力来了：在用户程序写好之后，如果需要替换无人机机架的话，仅需简单的修改一下机架配置参数即可，高层的用户自定义飞控应用几乎无需修改。理解上述初衷至关重要。有很多搞自动化出身、没太多软件经验的朋友倾向于直接使用底层控制协议来控制飞控板，但实际上PX4架构已经在更高的抽象层面上提供了更好的选择，无论是代码维护成本、开发效率、硬件兼容性都能显著高于前者。很多支持前者方式的开发者的理由主要在于高层封装机制效率较低，而飞控板性能不够，容易给飞控板造

4、成较大的处理负载，但实际从个人感觉上来看，遵循PX4的软件架构模式反倒更容易实现较高处理性能，不容易产生控制拥塞，提升无人机侧系统的并发处理效率。2、PX4/Pixhawk飞行控制协议与逻辑Mavlink是目前最常见的无人机飞控协议之一。PX4对Mavlink协议提供了良好的原生支持。该协议既可以用于地面站(GCS)对无人机(UAV)的控制，也可用于UAV对GCS的信息反馈。其飞控场景一般是这样的：a) 手工飞控：GCS -> (MavLink) -> UAVb) 信息采集：GCS <- (Mavlink) <- UAVc) 自治飞控：User App -> (M

5、avLink) -> UAV也就是说，如果你想实现地面站控制飞行，那么由你的地面站使用Mavlink协议，通过射频信道(或 wifi etc.)给无人机发送控制指令就可以了。如果你想实现无人机自主飞行，那么就由你自己写的应用（运行在无人机系统上）使用Mavlink协议给无人机发送本地的控制指令就可以了。然而，为实现飞控架构的灵活性，避免对底层实现细节的依赖，在PX4中，并不鼓励开发者在自定义飞控程序中直接使用Mavlink，而是鼓励开发者使用一种名为uORB(Micro Object Request Broker，微对象请求代理)的消息机制。其实uORB在概念上等同于posix里面的命名

6、管道(named pipe)，它本质上是一种进程间通信机制。由于PX4实际使用的是NuttX实时ARM系统，因此uORB实际上相当于是多个进程（驱动级模块）打开同一个设备文件，多个进程（驱动级模块）通过此文件节点进行数据交互和共享。在uORB机制中，交换的消息被称之为topic，一个topic仅包含一种message类型（即数据结构）。每个进程（或驱动模块）均可“订阅”或“发布”多个topic，一个topic可以存在多个发布者，而且一个订阅者可也订阅多个topic。而正因为有了uORB机制的存在，上述飞控场景变成了：a) 手工飞控：GCS -> (MavLink) -> (uORB

7、 topic) -> UAVb) 信息采集：GCS <- (Mavlink) <- (uORB topic) <- UAVc) 自治飞控：User App -> (uORB topic) -> (MavLink) -> UAV有了以上背景基础，便可以自写飞控逻辑了，仅需在PX4源码中，添加一个自定义module，然后使用uORB订阅相关信息（如传感器消息等），并发布相关控制信息（如飞行模式控制消息等）即可。具体的uORB API、uORB消息定义可参考PX4文档与源码，所有控制命令都在firmware代码的msg里面，不再敷述。最后值得一提的是，在PX

8、4系统中，还提供了一个名为mavlink的专用module，源码在firmware的src/modules/mavlink中，这货与linux的控制台命令工具集相当相似，其既可以作为ntt控制台下的命令使用，又可作为系统模块加载后台运行。其所实现的功能包括：1）uORB消息解析，将uORB消息实际翻译为具体的Mavlink底层指令，或反之。2）通过serial/射频通信接口获取或发送Mavlink消息，既考虑到了用户自写程序的开发模式，也适用于类似linux的脚本工具链开发模式，使用起来很灵活，有兴趣的可以看看。PX4飞控中利用EKF估计姿态角代码详解PX4飞控中主要用EKF算法来估计飞行器三

9、轴姿态角，具体c文件在px4Firmwaresrcmodulesattitude_estimator_ekfcodegen目录下· 具体原理· 程序详解· 下一步1.具体原理EKF算法原理不再多讲，具体可参见上一篇blog 这篇讲EKF算法执行过程，需要以下几个关键式子：· 飞行器状态矩阵： 这里表示三轴角速度，表示三轴角加速度，表示加速度在机体坐标系三轴分量，表示磁力计在机体坐标系三轴分量。· 测量矩阵 分别由三轴陀螺仪，加速度计，磁力计测得。· 状态转移矩阵：飞行器下一时刻状态预测矩阵如下：其中W项均

10、为高斯噪声， 为飞行器在姿态发生变化后，坐标系余旋变换矩阵，对该函数在处求一阶偏导，可得到状态转移矩阵：此时可得到飞行器状态的先验估计：· 利用测量值修正先验估计：这里测量矩阵H与状态矩阵X为线性关系，故无需求偏导。卡尔曼增益：状态后验估计：方差后验估计：2.程序详解整个EKF的代码挺长的，大部分是矩阵运算，而且使用嵌套for循环来执行的，所以读起来比较费劲，但是要是移植到自己工程上的话必然离不开这一步，所以花了一个下午把各个细节理清楚，顺便记录分享。/* Include files */#include "rt_nonfinite.h"#include

11、 "attitudeKalmanfilter.h"#include "rdivide.h"#include "norm.h"#include "cross.h"#include "eye.h"#include "mrdivide.h"/* '输入参数：updateVect3：用来记录陀螺仪，加速度计，磁力计传感器数值是否有效 z9 ：测量矩阵 x_aposteriori_k12：上一时刻状态后验估计矩阵，用来预测当前状态 P_aposteriori_k144：上一时刻

12、后验估计方差 eulerAngles3 ：输出欧拉角 Rot_matrix9 ：输出余弦矩阵 x_aposteriori12 ：输出状态后验估计矩阵 P_aposteriori144 ：输出方差后验估计矩阵'*/void attitudeKalmanfilter(const uint8_T updateVect3,real32_T dt, const real32_T z9, const real32_T x_aposteriori_k12, const real32_T P_aposteriori_k144, const real32_T q12, real32_T r9, real

13、32_T eulerAngles3, real32_T Rot_matrix9,real32_T x_aposteriori12, real32_T P_aposteriori144)/*以下这一堆变量用到的时候再解释*/ real32_T wak3; real32_T O9; real_T dv09; real32_T a9; int32_T i; real32_T b_a9; real32_T x_n_b3; real32_T b_x_aposteriori_k3; real32_T z_n_b3; real32_T c_a3; real32_T d_a3; int32_T i0; rea

14、l32_T x_apriori12; real_T dv1144; real32_T A_lin144; static const int8_T iv036 = 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T b_A_lin144; real32_T b_q144; real32_T c_A_lin144; real32_T d_A_lin144; real32_T e_A_lin144; int32_T i

15、1; real32_T P_apriori144; real32_T b_P_apriori108; static const int8_T iv1108 = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0

16、, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; real32_T K_k108; real32_T fv081; static const int8_T iv2108 = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0

17、, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; real32_T b_r81; real32_T fv181; real32_T f0; real32_T c_P_apriori36= 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

18、1, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T fv236; static const int8_T iv436 = 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T c_r9; real32_T b_K_k36; real32_T d_P_apriori72; static const int8_T iv572 = 1, 0, 0, 0, 0, 0, 0,

19、 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 ; real32_T c_K_k72; static const int8_T iv672 = 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0,

20、0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; real32_T b_z6; static const int8_T iv772 = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 ; static

21、 const int8_T iv872 = 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1 ; real32_T fv36; real32_T c_z6; /*开始计算*/ /*'wak为当前状态三轴角加速度'*/ wak0 = x_aposteriori_k3; wak1 = x_aposteriori_k4; wak2 = x_aposte

22、riori_k5;· 1· 2· 3· 4· 5· 6· 7· 8· 9· 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33

23、83; 34· 35· 36· 37· 38· 39· 40· 41· 42· 43· 44· 45· 46· 47· 48· 49· 50· 51· 52· 53· 54· 55· 56· 57· 58· 59· 60· 61· 62· 63· 64· 65· 66·

24、 67· 68· 69· 70· 71· 72· 73· 74· 75· 76· 77· 78· 79· 80· 81· 82· 83· 84· 85· 86· 87· 88· 89· 90· 91· 92· 93· 94· 95· 96· 97· 98· 99· 10

25、0· 101· 102· 103· 104· 105· 106· 107· 108· 109· 110· 111· 112· 113· 114· 115· 116· 117· 118· 119· 120· 121· 122· 123· 124· 125· 126· 127· 128· 129· 13

26、0· 131· 132· 133· 134· 135· 136· 137· 138· 139· 140· 141· 142· 143· 144· 145· 146· 147· 148· 149· 150· 151· 152· 153· 154· 155· 156· 157· 158· 159· 16

27、0· 161· 162/* 欧拉角旋转矩阵 O=0wzwywz0wxwywx0这里的O矩阵相当于A矩阵中的的转置矩阵！ */ O0 = 0.0F; O1 = -x_aposteriori_k2; O2 = x_aposteriori_k1; O3 = x_aposteriori_k2; O4 = 0.0F; O5 = -x_aposteriori_k0; O6 = -x_aposteriori_k1; O7 = x_aposteriori_k0; O8 = 0.0F; /* 预测转过一个小角度之后的重力向量三轴投影 */ /* a = 1, -delta

28、_z, delta_y; * delta_z, 1 , -delta_x; * -delta_y, delta_x, 1 ' */ eye(dv0); /dv0矩阵单位化 for (i = 0; i < 9; i+) ai = (real32_T)dv0i + Oi * dt; /* 预测转过一个小角度之后的磁力向量三轴投影 */ eye(dv0); for (i = 0; i < 9; i+) b_ai = (real32_T)dv0i + Oi * dt; · 1· 2· 3· 4· 5· 6· 7&

29、#183; 8· 9· 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28/*a=1zyz1xyx1其实就是这个大家都很眼熟的的余弦矩阵的转置, 用来更新机体转过一个角度之后的重力和磁力三轴投影，只不过两次计算间隔时间很短，变化角度很小，因此忽略高阶小量之后就变成了这个样子。这里还少一个时间系数dt，

30、下面会补上。 cosycoszcosysinzsiny(sinxsinycosz)+(cosxsinz)(sinxsinysinz)+(cosxcosz)sinxcosy(cosxsinycosz)+(sinxsinz)(cosxsinysinz)+(sinxcosz)cosxcosy*/ x_n_b0 = x_aposteriori_k0; /角速度 x_n_b1 = x_aposteriori_k1; x_n_b2 = x_aposteriori_k2; b_x_aposteriori_k0 = x_aposteriori_k6; / 加速度 b_x_aposteriori_k1

31、 = x_aposteriori_k7; b_x_aposteriori_k2 = x_aposteriori_k8; z_n_b0 = x_aposteriori_k9; /磁力计 z_n_b1 = x_aposteriori_k10; z_n_b2 = x_aposteriori_k11; for (i = 0; i < 3; i+) c_ai = 0.0F; for (i0 = 0; i0 < 3; i0+) c_ai += ai + 3 * i0 * b_x_aposteriori_ki0; d_ai = 0.0F; for (i0 = 0; i0 < 3; i0+)

32、 d_ai += b_ai + 3 * i0 * z_n_bi0; x_apriorii = x_n_bi + dt * waki; for (i = 0; i < 3; i+) x_apriorii + 3 = waki; for (i = 0; i < 3; i+) x_apriorii + 6 = c_ai; for (i = 0; i < 3; i+) x_apriorii + 9 = d_ai; /得到状态先验估计 · 1· 2· 3· 4· 5· 6· 7· 8· 9

33、3; 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33· 34/* 根据上述矩阵运算，可以得到： c_a13=axayaza33从而：kra,kt31=c_a13Td_a13=mxmymza33从而：

34、krm,kt31=d_a13T其中axayaz和mxmymz分别对应ra,k和rm,k的转置；得到状态先验估计: Xk+1121=x_apriori112T=x_n_b+wakdtwakc_ad_aT*/* '开始计算A矩阵'*/ b_eye(dv1); for (i = 0; i < 12; i+) for (i0 = 0; i0 < 3; i0+) A_lini0 + 12 * i = (real32_T)iv0i0 + 3 * i; /*1 2 3列*/ for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * i)

35、+ 3 = 0.0F; /*3 4 5列*/ /*6 7 8 列*/ A_lin6 = 0.0F; A_lin7 = x_aposteriori_k8; A_lin8 = -x_aposteriori_k7; A_lin18 = -x_aposteriori_k8; A_lin19 = 0.0F; A_lin20 = x_aposteriori_k6; A_lin30 = x_aposteriori_k7; A_lin31 = -x_aposteriori_k6; A_lin32 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3;

36、 i0+) A_lin(i0 + 12 * (i + 3) + 6 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 6) + 6 = Oi0 + 3 * i; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 9) + 6 = 0.0F; /*6 7 8 列*/ /*9 10 11 列*/ A_lin9 = 0.0F; A_lin10 = x_aposteriori_k11;

37、 A_lin11 = -x_aposteriori_k10; A_lin21 = -x_aposteriori_k11; A_lin22 = 0.0F; A_lin23 = x_aposteriori_k9; A_lin33 = x_aposteriori_k7; A_lin34 = -x_aposteriori_k9; A_lin35 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 3) + 9 = 0.0F; for (i = 0; i < 3; i+) for (

38、i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 6) + 9 = 0.0F; for (i = 0; i < 3; i+) for (i0 = 0; i0 < 3; i0+) A_lin(i0 + 12 * (i + 9) + 9 = Oi0 + 3 * i; /*9 10 11 列*/· 1· 2· 3· 4· 5· 6· 7· 8· 9· 10· 11· 12· 13· 14· 15

39、83; 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33· 34· 35· 36· 37· 38· 39· 40· 41· 42· 43· 44· 45· 46· 47· 48·

40、 49· 50· 51· 52· 53· 54· 55· 56· 57· 58· 59· 60· 61· 62· 63· 64· 65· 66· 67· 68/* 根据上述矩阵运算，可以得到A_lin矩阵： A_lin1212=0I000000A10O0A200O其中各元素都是3*3的矩阵；I为单位矩阵，其中 A1=0azayaz0axayax0=ra,Tk同样A2=0mzmy

41、mz0mxmymx0=rm,Tk*/ for (i = 0; i < 12; i+) for (i0 = 0; i0 < 12; i0+) b_A_lini0 + 12 * i = (real32_T)dv1i0 + 12 * i + A_lini0 + 12 * i *dt; /最终A_link,k的逆矩阵· 1· 2· 3· 4· 5得到： Alin,Tk=b_A_lin1212=I0000I0000I0000I+0I000000A10O0A200Odt/* 开始根据计算过程方差 */ 过

42、程噪声方差b_q1212=q00000q10000q20000q3其中各元素都是3*3的矩阵； b_q0 = q0; b_q12 = 0.0F; b_q24 = 0.0F; b_q36 = 0.0F; b_q48 = 0.0F; b_q60 = 0.0F; b_q72 = 0.0F; b_q84 = 0.0F; b_q96 = 0.0F; b_q108 = 0.0F; b_q120 = 0.0F; b_q132 = 0.0F; b_q1 = 0.0F; b_q13 = q0; b_q25 = 0.0F; b_q37 = 0.0F; b_q49 = 0.0F; b_q61 = 0.0F; b_q

43、73 = 0.0F; b_q85 = 0.0F; b_q97 = 0.0F; b_q109 = 0.0F; b_q121 = 0.0F; b_q133 = 0.0F; b_q2 = 0.0F; b_q14 = 0.0F; b_q26 = q0; b_q38 = 0.0F; b_q50 = 0.0F; b_q62 = 0.0F; b_q74 = 0.0F; b_q86 = 0.0F; b_q98 = 0.0F; b_q110 = 0.0F; b_q122 = 0.0F; b_q134 = 0.0F; b_q3 = 0.0F; b_q15 = 0.0F; b_q27 = 0.0F; b_q39 =

44、 q1; b_q51 = 0.0F; b_q63 = 0.0F; b_q75 = 0.0F; b_q87 = 0.0F; b_q99 = 0.0F; b_q111 = 0.0F; b_q123 = 0.0F; b_q135 = 0.0F; b_q4 = 0.0F; b_q16 = 0.0F; b_q28 = 0.0F; b_q40 = 0.0F; b_q52 = q1; b_q64 = 0.0F; b_q76 = 0.0F; b_q88 = 0.0F; b_q100 = 0.0F; b_q112 = 0.0F; b_q124 = 0.0F; b_q136 = 0.0F; b_q5 = 0.0F

45、; b_q17 = 0.0F; b_q29 = 0.0F; b_q41 = 0.0F; b_q53 = 0.0F; b_q65 = q1; b_q77 = 0.0F; b_q89 = 0.0F; b_q101 = 0.0F; b_q113 = 0.0F; b_q125 = 0.0F; b_q137 = 0.0F; b_q6 = 0.0F; b_q18 = 0.0F; b_q30 = 0.0F; b_q42 = 0.0F; b_q54 = 0.0F; b_q66 = 0.0F; b_q78 = q2; b_q90 = 0.0F; b_q102 = 0.0F; b_q114 = 0.0F; b_q

46、126 = 0.0F; b_q138 = 0.0F; b_q7 = 0.0F; b_q19 = 0.0F; b_q31 = 0.0F; b_q43 = 0.0F; b_q55 = 0.0F; b_q67 = 0.0F; b_q79 = 0.0F; b_q91 = q2; b_q103 = 0.0F; b_q115 = 0.0F; b_q127 = 0.0F; b_q139 = 0.0F; b_q8 = 0.0F; b_q20 = 0.0F; b_q32 = 0.0F; b_q44 = 0.0F; b_q56 = 0.0F; b_q68 = 0.0F; b_q80 = 0.0F; b_q92 =

47、 0.0F; b_q104 = q2; b_q116 = 0.0F; b_q128 = 0.0F; b_q140 = 0.0F; b_q9 = 0.0F; b_q21 = 0.0F; b_q33 = 0.0F; b_q45 = 0.0F; b_q57 = 0.0F; b_q69 = 0.0F; b_q81 = 0.0F; b_q93 = 0.0F; b_q105 = 0.0F; b_q117 = q3; b_q129 = 0.0F; b_q141 = 0.0F; b_q10 = 0.0F; b_q22 = 0.0F; b_q34 = 0.0F; b_q46 = 0.0F; b_q58 = 0.

48、0F; b_q70 = 0.0F; b_q82 = 0.0F; b_q94 = 0.0F; b_q106 = 0.0F; b_q118 = 0.0F; b_q130 = q3; b_q142 = 0.0F; b_q11 = 0.0F; b_q23 = 0.0F; b_q35 = 0.0F; b_q47 = 0.0F; b_q59 = 0.0F; b_q71 = 0.0F; b_q83 = 0.0F; b_q95 = 0.0F; b_q107 = 0.0F; b_q119 = 0.0F; b_q131 = 0.0F; b_q143 = q3; for (i = 0; i < 12; i+)

49、 for (i0 = 0; i0 < 12; i0+) A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) A_lini + 12 * i0 += b_A_lini + 12 * i1 * P_aposteriori_ki1 + 12 *i0; c_A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) c_A_lini + 12 * i0 += b_A_lini + 12 * i1 * b_qi1 + 12 * i0; for (i0 = 0; i0 < 12; i0+

50、) d_A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) d_A_lini + 12 * i0 += A_lini + 12 * i1 * b_A_lini0 + 12 * i1; e_A_lini + 12 * i0 = 0.0F; for (i1 = 0; i1 < 12; i1+) e_A_lini + 12 * i0 += c_A_lini + 12 * i1 * b_A_lini0 + 12 * i1; · 1· 2· 3· 4· 5· 6· 7&#

51、183; 8· 9· 10· 11· 12· 13· 14· 15· 16· 17· 18· 19· 20· 21· 22· 23· 24· 25· 26· 27· 28· 29· 30· 31· 32· 33· 34· 35· 36· 37· 38· 39· 40· 41· 42· 43· 44· 45· 46· 47