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=(a*nx)/N,wy=(a*ny)/N。假设飞机是不动的,地面在动,第一帧特征点在A1,第二帧特征点在A2,则视场角转动了wx,wy。设地面移动速度是vx,vy。由弧长等于半径乘以弧度。所以wx =vx/H,wy=vy/H。这里会有正负号变化,甚至wx,wy会互换,要看像素点坐标和机体坐标的关系。
融合光流传感器,我们对应的状态量是速度,而光流传感器测得是视场角速度。所以他跟磁力计是一样的。