请教一下移植过PIX代码的大神关于里面EKF的相关问题。

makeflyeasy

小弟以前用的别的姿态测算程序，最近想试试PIX的EKF程序，也就是EKF_att_pos，但是移植完毕后发现输出的姿态和加速度无关，也就是加速度根本就不参与姿态融合计算，出来的姿态纯粹是用陀螺仪积分积出来的，通读了整个PIX的姿态融合代码，都找不到EKF里面加速度值在哪里参与了姿态融合。
小弟先讲下我对PIX代码EKF部分的理解吧，麻烦众位大神看看对不对。

首先是EKF参数初始化，包括各种噪声的预设值。

这个图中的值我已经按照代码里面的推荐值初始化了。


之后是如果EKF没有初始化，则初始化它。这个也已经做了。

其次就是EKF主程序了，流程是填数据(polldata)  运行EKF主程序，主程序又包括惯导方程更新，也就是六轴数据更新，这个是必须的，然后这些是可选的:GPS融合，磁力计融合，光流融合，气压计融合，测距仪融合。
之后就是发布各种数据。 整个EKF就完了，EKF和发布数据中间有个地形更新不知道是干啥用的。


我是这么移植的，这是我的进程循环过程:

首先是获取九轴数据
其次是填充数据：

再者就运行惯导方程更新，但是问题来了，加速度计的值，这里没有直接用到，用到的是Acc*dt，也就是dAngIMU；
但是推演下去，发现加速度的值在EKF主程序里面根本就没有融合到最终姿态里面去，也就是没有用加速度计的值修正最终姿态。

可以看到dVelIMURel = dVelIMU;,这个唯一和加速度有关的参数,在四元数归一化并转化为最终四元数输出时，都没有参与姿态融合运算。


之后dVelIMURel 参与了delVelNav参数的运算，但是这个参数看后面可以知道是加速度积分算速度用的，根本就没有参与姿态融合。然后这个程序就完了，EKF主程序部分又是对GPS等数据融合的部分，又没有加速度直接参与。


这样就导致了我的结果，那就是姿态数据虽然出来了，但是是纯陀螺仪积分积出来的，加速度计没有参与姿态修正。  不过我将代码下到PIX的硬件里面姿态又是正常的，我移植后又是不正常的。

我的EKF周期是0.002s，dtIMU什么的我都改过，确认没有问题，想请教下各位大神，PIX的EKF里面加速度到底是怎么参与姿态融合运算的？是不是我哪里配置错了导致加速度没有参与姿态运算？

以下是我的程序截取:
这是EKF参数初始化

1. dAngBiasSigma = 1e-07f;//_parameters.gbias_pnoise;
   
2.         dVelBiasSigma = 1e-05f;//_parameters.abias_pnoise;
   
3.         magEarthSigma = 0.0003f;//_parameters.mage_pnoise;
   
4.         magBodySigma  = 0.0003f;//_parameters.magb_pnoise;
   
5.   //gndHgtSigma  = 0.02f;
   
6.         vneSigma = 0.3f;//_parameters.velne_noise;
   
7.         vdSigma = 0.3f;//_parameters.veld_noise;
   
8.         posNeSigma = 0.5f;//_parameters.posne_noise;
   
9.         posDSigma = 1.25f;//_parameters.posd_noise;
   
10.         magMeasurementSigma = 0.05f;//_parameters.mag_noise;
    
11.         gyroProcessNoise = 0.015f;//_parameters.gyro_pnoise;
    
12.         accelProcessNoise = 0.125f;//_parameters.acc_pnoise;
    
13.         airspeedMeasurementSigma = 1.4f;//_parameters.eas_noise;
    
14.         rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF

复制代码

这是数据输入:polldata  有乱码，请无视  这里陀螺仪数据的单位是弧度/s   加速度计数据单位是g

1. dAngIMU.x = 0.5f * (angRate.x + X_Gyro*0.0174533f) * dtIMU;
   
2.         dAngIMU.y = 0.5f * (angRate.y + Y_Gyro*0.0174533f) * dtIMU;
   
3.         dAngIMU.z = 0.5f * (angRate.z + Z_Gyro*0.0174533f) * dtIMU;
   
4.         
   
5.         angRate.x=X_Gyro*0.0174533f; //ÊäÈë½ÇËÙ¶È,µ¥Î»:rad/s
   
6.         angRate.y=Y_Gyro*0.0174533f; //0.174533ΪPI/180 Ä¿µÄÊǽ«½Ç¶Èת»¡¶È
   
7.         angRate.z=Z_Gyro*0.0174533f;
   
8.         
   
9.         dVelIMU.x = 0.5f * (accel.x + X_Accel) * dtIMU;
   
10.         dVelIMU.y = 0.5f * (accel.y + Y_Accel) * dtIMU;
    
11.         dVelIMU.z = 0.5f * (accel.z + Z_Accel) * dtIMU;
    
12.         
    
13.   accel.x=X_Accel; //ÊäÈë¼ÓËÙ¶È,µ¥Î»:g
    
14.         accel.y=Y_Accel;
    
15.         accel.z=Z_Accel;
    
16.         
    
17.         magData.x=MAG_X;  //ÊäÈë´ÅÁ¦¼ÆÊý¾Ý
    
18.         magData.y=MAG_Y;
    
19.         magData.z=MAG_Z;

复制代码

这是更新惯导方程的，和PIX的代码是一样的了，我觉得加速度是在这里面参与姿态融合的，但是找不到在哪里。

1. void UpdateStrapdownEquationsNED(void)
   
2. {
   
3.     Vector3f delVelNav;
   
4.           static float accNavMagHorizontal;
   
5.     float q00;
   
6.     float q11;
   
7.     float q22;
   
8.     float q33;
   
9.     float q01;
   
10.     float q02;
    
11.     float q03;
    
12.     float q12;
    
13.     float q13;
    
14.     float q23;
    
15.     float rotationMag;
    
16.     float qUpdated[4];
    
17.     float quatMag;
    
18.     float deltaQuat[4];
    
19.     const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS);
    
20. 
    
21.           if(First_Init_EKF==true)
    
22.                 {
    
23.                         float initVelNED[3] = {0.0f, 0.0f, 0.0f};
    
24.             InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
    
25.             posNE[0] = 0.0f;
    
26.             posNE[1] = 0.0f;
    
27.                 }
    
28.     // 删除传感器偏置错误
    
29.     correctedDelAng.x = dAngIMU.x - states[10];
    
30.     correctedDelAng.y = dAngIMU.y - states[11];
    
31.     correctedDelAng.z = dAngIMU.z - states[12];
    
32. 
    
33.     Vector3f dVelIMURel;
    
34. 
    
35.     dVelIMURel.x = dVelIMU.x;
    
36.     dVelIMURel.y = dVelIMU.y;
    
37.     dVelIMURel.z = dVelIMU.z - states[13];
    
38. 
    
39.     delAngTotal.x += correctedDelAng.x;
    
40.     delAngTotal.y += correctedDelAng.y;
    
41.     delAngTotal.z += correctedDelAng.z;
    
42. 
    
43.     // Apply corrections for earths rotation rate and coning errors
    
44.     // * and + operators have been overloaded
    
45.     correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
    
46.     prevDelAng = correctedDelAng;
    
47. 
    
48.     // 转化为等效四元数旋转矢量
    
49.     rotationMag = correctedDelAng.length();
    
50.     if (rotationMag < 1e-12f)
    
51.     {
    
52.         deltaQuat[0] = 1.0;
    
53.         deltaQuat[1] = 0.0;
    
54.         deltaQuat[2] = 0.0;
    
55.         deltaQuat[3] = 0.0;
    
56.     }
    
57.     else
    
58.     {
    
59.         // We are using double here as we are unsure how small
    
60.         // the angle differences are and if we get into numeric
    
61.         // issues with float. The runtime impact is not measurable
    
62.         // for these quantities.
    
63.         deltaQuat[0] = cos(0.5*(double)rotationMag);
    
64.         float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag;
    
65.         deltaQuat[1] = correctedDelAng.x*rotScaler;
    
66.         deltaQuat[2] = correctedDelAng.y*rotScaler;
    
67.         deltaQuat[3] = correctedDelAng.z*rotScaler;
    
68.     }
    
69. 
    
70.     // Update the quaternions by rotating from the previous attitude through
    
71.     // the delta angle rotation quaternion
    
72.     qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
    
73.     qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
    
74.     qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
    
75.     qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
    
76. 
    
77.     // Normalise the quaternions and update the quaternion states
    
78.     quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
    
79.     if (quatMag > 1e-16f)
    
80.     {
    
81.         float quatMagInv = 1.0f/quatMag;
    
82.         states[0] = quatMagInv*qUpdated[0];
    
83.         states[1] = quatMagInv*qUpdated[1];
    
84.         states[2] = quatMagInv*qUpdated[2];
    
85.         states[3] = quatMagInv*qUpdated[3];
    
86.     }
    
87.     // Calculate the body to nav cosine matrix
    
88.     q00 = sq(states[0]);
    
89.     q11 = sq(states[1]);
    
90.     q22 = sq(states[2]);
    
91.     q33 = sq(states[3]);
    
92.     q01 =  states[0]*states[1];
    
93.     q02 =  states[0]*states[2];
    
94.     q03 =  states[0]*states[3];
    
95.     q12 =  states[1]*states[2];
    
96.     q13 =  states[1]*states[3];
    
97.     q23 =  states[2]*states[3];
    
98. 
    
99.     Tbn.x.x = q00 + q11 - q22 - q33;
    
100.     Tbn.y.y = q00 - q11 + q22 - q33;
     
101.     Tbn.z.z = q00 - q11 - q22 + q33;
     
102.     Tbn.x.y = 2*(q12 - q03);
     
103.     Tbn.x.z = 2*(q13 + q02);
     
104.     Tbn.y.x = 2*(q12 + q03);
     
105.     Tbn.y.z = 2*(q23 - q01);
     
106.     Tbn.z.x = 2*(q13 - q02);
     
107.     Tbn.z.y = 2*(q23 + q01);
     
108. 
     
109.     Tnb = Tbn.transpose();
     
110. 
     
111.     // transform body delta velocities to delta velocities in the nav frame
     
112.     // * and + operators have been overloaded
     
113.     //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
     
114.     delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
     
115.     delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
     
116.     delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
     
117. 
     
118.     // calculate the magnitude of the nav acceleration (required for GPS
     
119.     // variance estimation)
     
120.     accNavMag = delVelNav.length()/dtIMU;
     
121. 
     
122.     //First order low-pass filtered magnitude of horizontal nav acceleration
     
123.     Vector3f derivativeNav = (delVelNav / dtIMU);
     
124.     float derivativeVelNavMagnitude = sqrtf(sq(derivativeNav.x) + sq(derivativeNav.y));
     
125.     accNavMagHorizontal = accNavMagHorizontal * 0.95f + derivativeVelNavMagnitude * 0.05f;
     
126. 
     
127.     // If calculating position save previous velocity
     
128.     float lastVelocity[3];
     
129.     lastVelocity[0] = states[4];
     
130.     lastVelocity[1] = states[5];
     
131.     lastVelocity[2] = states[6];
     
132. 
     
133.     // Sum delta velocities to get velocity
     
134.     states[4] = states[4] + delVelNav.x;
     
135.     states[5] = states[5] + delVelNav.y;
     
136.     states[6] = states[6] + delVelNav.z;
     
137. 
     
138.     // If calculating postions, do a trapezoidal integration for position
     
139.     states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
     
140.     states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
     
141.     states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
     
142. 
     
143.     // Constrain states (to protect against filter divergence)
     
144.     ConstrainStates();
     
145. 
     
146.     // update filtered IMU time step length
     
147.     dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
     
148. }

复制代码

makeflyeasy

啊，有木有人看得懂啊，我把PIX的EKF代码通读好几遍了，就是不知道加速度计怎么参与姿态融合了，不像以前的互补滤波程序，一眼就看出了加速度计是怎么融合进去的了。

makeflyeasy

只有自己顶喽

makeflyeasy

哎，看来是没人懂这玩意的原理了，用EKF2算了。

makeflyeasy

再顶顶，真的就没有人研究过PIX的扩展卡尔曼算法吗？

小李非刀

没研究过呢，帮顶，等高人。。。。

熵之矢

你看的是pixhawk的stack还是APM的？

笑笑我笑了

我看的pixhawk中的姿态估计有个.m文件，很容易就搞懂了啊，没LZ那么复杂。