23阶扩展卡尔曼求姿态、位置、速度

23阶卡尔曼总结。

一、首先给出卡尔曼的五个公式：

卡尔曼算法的本质为：根据上一刻的最优值估计此刻的预测值，实际测量此刻的测量值。将预测值和测量值加权和即此刻的最优值。

首先离散状态空间表达式为：

1. 根据上一刻估计此刻的预测值：
   
   P为估计误差协方差矩阵，协方差矩阵为X各个元素之间的协方差值组成的矩阵。

2. 求卡尔曼增益，即加权系数。

Kg= P(k|k-1)HT/(HP(k|k-1)HT+R)

R为W1的协方差矩阵,Q为W2的协方差矩阵。

3. 求此刻的最优值：
   
   R是表征测量误差的，R越大说明测量越不准，Kg=P/(P+R)，Kg是关于R的减函数，也就是R越大，Kg越小，Z占得权重越小。也就是当我们认为测量有误差，想减小权重时，我们应该加大R。Kg=(P+Q)/(P+Q+R),Q是表征处理误差。在PX4里面，Q一般很小。我们主要是调R。比如融合位置时，如果想相信GPS，则让R小一点，如果想偏重加速度计，则让R大一点。

http://blog.chinaunix.net/uid-26694208-id-3184442.html

二、23阶卡尔曼

1.23个状态为：

  stateVector = [q0;q1;q2;q3;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dvz_b;vwn;vwe;magN;magE;magD;magX;magY;magZ;ptd];

分别为四元数、NED velocity - m/sec、NED position - m、delta angle bias - rad、delta velocity bias - m/sec、NE wind velocity - m/sec、NED earth fixed magnetic field components - milligauss、XYZ body fixed magnetic field measurements - milligauss、location of terrain in D axis。

2.预测此刻的状态值。

A．四元数采用的一阶龙格库塔方程更新。

   Q = Q + 0.5Q※w*T = Q + 0.5Q※dAngCor（w角速度，dAngCor为减去偏差后姿态角增量）

B．速度更新

   vNew = [vn;ve;vd] + [gn;ge;gd]*dt + Tbn*dVelCor;

本质为：V=V0+at dVelCor是机体测量的加速度乘以T，而测量的加速度是包含了重力加速度的，要把它消掉。飞机放在地上不动，测量值为[0,0,-g],刚好自由下落时，测量值为[0,0,0]。所以要加上[gn;ge;gd]*dt啦。

C．位置更新。

  pNew = [pn;pe;pd] + [vn;ve;vd]*dt;

本质为：S=S0+Vt

D．其他的更新认为还是原来的值。

vwnNew = vwn; vweNew = vwe; magNnew = magN; magEnew = magE; magDnew = magD; magXnew = magX; magYnew = magY; magZnew = magZ;

3.预测协方差矩阵。

processEqns = [qNew;vNew;pNew;dabNew;dvbNew;vwnNew;vweNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;ptdNew];

F = jacobian(processEqns, stateVector);

[F,SF]=OptimiseAlgebra(F,'SF');

F是更新后的状态对状态量求偏导。即

分析F。F是23X23阶，很大，但很多元素为0。F的第一行q0new对所有状态逐一求导，第二行是q1new对所有状态逐一求导，依次类推。所以F的对角元素全部为1（自己对自己求导嘛），两个状态之间没有关系的，求导肯定就为0嘛。第一行，q0对所有状态求偏导，q0的更新是由q1,q2,q3,角度偏差决定的，所以[0][0], [0][1]，[0][2], [0][3], [0][10], [0][11], [0][12]有值，其余为0。同理[1][0], [1][1]，[1][2], [1][3], [1][10], [1][11], [1][12]有值，[2][0], [2][1]，[2][2], [2][3], [2][10], [2][11], [2][12]有值，[3][0], [3][1]，[3][2], [3][3], [3][10], [3][11], [3][12]有值。Vn的更新与q0,q1,q2,q3,vn,dvz_b有关，所以[4][0], [4][1], [4][2], [4][3], [4][4], [4][13]有值。Ve的更新与q0,q1,q2,q3,ve,dvz_b有关，所以[5][0], [5][1], [5][2], [5][3], [5]5], [5][13]有值。Vd的更新与q0,q1,q2,q3,vd,dvz_b有关，所以[6][0], [6][1], [6][2], [6][3], [6]6], [6][13]有值.pn的更新即pn=pn+vndt,所以它的更新由pn，vn决定，所以[7][4],[7][7]有值。同理，

[8][5],[8][8], [9][6],[9][9]有值。好了，说这么多，只想一闭眼就能想到F矩阵的大致情况。

4.求P。P=FPFT+Q。

F上面已经求出。P的初始值我们一般取对角阵。就只需要求Q了。Q是测量噪声的协方差。有时我们取得是定值。所以P更新是一个纯数学计算。另外协方差矩阵的元素是状态量之间的协方差，表征了状态量之间的相关性。

P初值取对角阵，FPFT得到的矩阵值为F的元素平方，然后每行元素分别乘以P对角阵元素。这样我们就可以确定理论上F为0的那些元素，预测更新的P对应元素也是0.Q也是对角阵。

5.上面预测更新了X和P。我们的卡尔曼融合了多种传感器，不同的传感器，就有不同的观测值Z。所以接下来的三个更新方程每个传感器融合都是不同的。比如融合磁力计。

void AttPosEKF::FuseMagnetometer()

{

    float &q0 = magstate.q0;   这是定义的引用

    float &q1 = magstate.q1;

    float &q2 = magstate.q2;

    float &q3 = magstate.q3;

    float &magN = magstate.magN;

    float &magE = magstate.magE;

    float &magD = magstate.magD;

    float &magXbias = magstate.magXbias;

    float &magYbias = magstate.magYbias;

    float &magZbias = magstate.magZbias;

    unsigned &obsIndex = magstate.obsIndex;

    Mat3f &DCM = magstate.DCM;

    float *MagPred = &magstate.MagPred[0];

    float &R_MAG = magstate.R_MAG;

    float *SH_MAG = &magstate.SH_MAG[0];

    float SK_MX[6];

    float SK_MY[5];

    float SK_MZ[6];

    float H_MAG[n_states];

    for (uint8_t i = 0; i < n_states; i++) {

        H_MAG[i] = 0.0f;

    }

    if (useCompass && fuseMagData && (obsIndex < 3))

    {

        // Calculate observation jacobians and Kalman gains

        if (obsIndex == 0)

        {

            // Copy required states to local variable names

            q0       = statesAtMagMeasTime[0];      把上一刻的状态拿出来给它们，

            q1       = statesAtMagMeasTime[1];

            q2       = statesAtMagMeasTime[2];

            q3       = statesAtMagMeasTime[3];

            magN     = statesAtMagMeasTime[16];

            magE     = statesAtMagMeasTime[17];

            magD     = statesAtMagMeasTime[18];

            magXbias = statesAtMagMeasTime[19];

            magYbias = statesAtMagMeasTime[20];

            magZbias = statesAtMagMeasTime[21];

 

            // rotate predicted earth components into body axes and calculate

            // predicted measurments

            DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3;

            DCM.x.y = 2*(q1*q2 + q0*q3);

            DCM.x.z = 2*(q1*q3-q0*q2);

            DCM.y.x = 2*(q1*q2 - q0*q3);

            DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3;

            DCM.y.z = 2*(q2*q3 + q0*q1);

            DCM.z.x = 2*(q1*q3 + q0*q2);

            DCM.z.y = 2*(q2*q3 - q0*q1);

            DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;

            MagPred[0] = DCM.x.x*magN + DCM.x.y*magE  + DCM.x.z*magD + magXbias;   

            MagPred[1] = DCM.y.x*magN + DCM.y.y*magE  + DCM.y.z*magD + magYbias;

            MagPred[2] = DCM.z.x*magN + DCM.z.y*magE  + DCM.z.z*magD + magZbias;

这里我们的状态量给的状态是地面坐标系下的磁场，而实际测量的是机体坐标系下的。所以要把地理坐标系下的磁场旋转到机体坐标系下，然后加上偏差，就是预测的机体坐标系下的磁场了。这里我看了好久，上面我们已经更新了X=FX了，状态量magN,magE,magD已经更新了，是magNnew = magN;这里千万不要搞混，这里只是把地面旋转到机体，并不是状态更新，不要被MagPred这个名字误导了。

R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU);

      // Calculate observation jacobians

            SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;

            SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;

            SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;

            SH_MAG[3] = sq(q3);

            SH_MAG[4] = sq(q2);

            SH_MAG[5] = sq(q1);

            SH_MAG[6] = sq(q0);

            SH_MAG[7] = 2*magN*q0;

            SH_MAG[8] = 2*magE*q3;

 

            for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0;

            H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;

            H_MAG[1] = SH_MAG[0];

            H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;

            H_MAG[3] = SH_MAG[2];

            H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];

            H_MAG[17] = 2*q0*q3 + 2*q1*q2;

            H_MAG[18] = 2*q1*q3 - 2*q0*q2;

            H_MAG[19] = 1.0f;

求H矩阵。磁力计融合他是分了三步做的，先融合X，再融合Y,再融合Z。这里是只融合X。求H矩阵，他用求得的机体坐标下的磁场MagPred[0]对q0,q1,q2,q3,magN,magD,magE,magXbias求偏导。按理说，求H应该是观测值对状态量求偏导啊，他这里居然是用预测值求得。我猜是这样的。磁力计与GPS融合是不同的。我们融合GPS时，H是单位矩阵，为什么呢？Z=HX，我们的状态是NED velocity - m/sec、NED position – m，是地理坐标系下的位置和速度，而我们GPS本身就是测得是地理坐标系下的位置和速度啊，所以H就是1.而磁力计就不同了，我们的状态是magN,magD,magE,是地理坐标系下的磁场，而我们的观测值却是磁力计测量的机体坐标系下的值。所以这里观测值和状态量是不对等的！！没有人说观测值一定要和状态值是同一个值啊！！所以我们要求出观测值和状态值之间的关系，即H矩阵啦。这里容易把人搞晕的是，既然是观测值对状态量就偏导，那就应该是测量值啊。拜托，测量值只是一个数字，怎么去求偏导呢？所以，老说的观测值对状态求偏导，实际是找出观测值与状态的理论关系，并不是真的用测量得到的那个数据去求偏导。这里就是把地理坐标下的磁场旋转得到机体坐标系下磁场（理论观测值），然后对状态求偏导，得到H矩阵。切记，Z=HX，这里Z也是理论的才能求H啊。我们这里Z就是MagPred[0]，MagPred[1]，MagPred[2]。

它先用MagPred[0]求偏导，求了H，然后按步骤，求Kg。求innovMag[0] = MagPred[0] - magData.x;这里就是X=X-Kg(HX - Z)中的HX – Z。磁力计融合是一个典型的状态量和观测量不同的卡尔曼滤波，又学习了！

它用磁力计的三个测量值，融合了三次，步骤是一模一样的。

光流融合：光流传感器的视场角是a，视场角对应的总的像素点是N，离地面高度是H。同一特征点在第一帧和第二帧移动的像素点差为nx，ny。则视场角速度为wx=(anx)/N，wy=(any)/N。假设飞机是不动的，地面在动，第一帧特征点在A1，第二帧特征点在A2，则视场角转动了wx,wy。设地面移动速度是vx,vy。由弧长等于半径乘以弧度。所以wx =vx/H，wy=vy/H。这里会有正负号变化，甚至wx,wy会互换，要看像素点坐标和机体坐标的关系。

融合光流传感器，我们对应的状态量是速度，而光流传感器测得是视场角速度。所以他跟磁力计是一样的。