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=(a*nx)/N，wy=(a*ny)/N。假設飛機是不動的，地面在動，第一幀特征點在A1，第二幀特征點在A2，則視場角轉動了wx,wy。設地面移動速度是vx,vy。由弧長等於半徑乘以弧度。所以wx =vx/H，wy=vy/H。這里會有正負號變化，甚至wx,wy會互換，要看像素點坐標和機體坐標的關系。
融合光流傳感器，我們對應的狀態量是速度，而光流傳感器測得是視場角速度。所以他跟磁力計是一樣的。