1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

Some small updates to 16 state INS

This commit is contained in:
James Cotton 2012-02-28 03:05:07 -06:00
parent 093d52b002
commit 1630267324

View File

@ -76,37 +76,37 @@ uint16_t ins_get_num_states()
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
Be[0] = 1.0f;
Be[1] = 0;
Be[2] = 0; // local magnetic unit vector
for (int i = 0; i < NUMX; i++) {
for (int j = 0; j < NUMX; j++) {
P[i][j] = 0; // zero all terms
P[i][j] = 0.0f; // zero all terms
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5; // initial gyro bias variance (rad/s)^2
P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2
P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5f; // initial quaternion variance
P[10][10] = P[11][11] = P[12][12] = 1e-5f; // initial gyro bias variance (rad/s)^2
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0; // initial pos and vel (m)
X[6] = 1;
X[7] = X[8] = X[9] = 0; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0; // initial gyro bias (rad/s)
X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0.0f; // initial pos and vel (m)
X[6] = 1.0f;
X[7] = X[8] = X[9] = 0.0f; // initial quaternion (level and North) (m/s)
X[10] = X[11] = X[12] = 0.0f; // initial gyro bias (rad/s)
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
Q[9] = Q[10] = Q[11] = 2e-20; // accel bias random walk variance (m/s^3)^2
Q[0] = Q[1] = Q[2] = 50e-8f; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01f; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9f; // gyro bias random walk variance (rad/s^2)^2
Q[9] = Q[10] = Q[11] = 2e-20f; // accel bias random walk variance (m/s^3)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
R[3] = R[4] = 0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5] = 100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005; // magnetometer unit vector noise variance
R[9] = .05; // High freq altimeter noise variance (m^2)
R[0] = R[1] = 0.004f; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036f; // High freq GPS vertical position noise variance (m^2)
R[3] = R[4] = 0.004f; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5] = 100.0f; // High freq GPS vertical velocity noise variance (m/s)^2
R[6] = R[7] = R[8] = 0.005f; // magnetometer unit vector noise variance
R[9] = .05f; // High freq altimeter noise variance (m^2)
}
void INSResetP(float PDiag[NUMX])
@ -117,7 +117,7 @@ void INSResetP(float PDiag[NUMX])
for (i=0;i<NUMX;i++){
if (PDiag != 0){
for (j=0;j<NUMX;j++)
P[i][j]=P[j][i]=0;
P[i][j]=P[j][i]=0.0f;
P[i][i]=PDiag[i];
}
}
@ -147,13 +147,13 @@ void INSPosVelReset(float pos[3], float vel[3])
{
for (int i = 0; i < 6; i++) {
for(int j = i; j < NUMX; j++) {
P[i][j] = 0; // zero the first 6 rows and columns
P[j][i] = 0;
P[i][j] = 0.0f; // zero the first 6 rows and columns
P[j][i] = 0.0f;
}
}
P[0][0] = P[1][1] = P[2][2] = 25; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5; // initial velocity variance (m/s)^2
P[0][0] = P[1][1] = P[2][2] = 25.0f; // initial position variance (m^2)
P[3][3] = P[4][4] = P[5][5] = 5.0f; // initial velocity variance (m/s)^2
X[0] = pos[0];
X[1] = pos[1];
@ -254,7 +254,7 @@ void INSCovariancePrediction(float dT)
CovariancePrediction(F, G, Q, dT, P);
}
float zeros[3] = { 0, 0, 0 };
float zeros[3] = { 0.0f, 0.0f, 0.0f };
void MagCorrection(float mag_data[3])
{
@ -573,7 +573,7 @@ void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
if (SensorsUsed & (0x01 << m)) { // use this sensor for update
for (j = 0; j < NUMX; j++) { // Find Hp = H*P
HP[j] = 0;
HP[j] = 0.0f;
for (k = 0; k < NUMX; k++)
HP[j] += H[m][k] * P[k][j];
}
@ -609,7 +609,7 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
{
float dT2 =
dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
dT / 2.0f, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
uint8_t i;
for (i = 0; i < NUMX; i++)
@ -629,8 +629,8 @@ void RungeKutta(float X[NUMX], float U[NUMU], float dT)
// Xnew = X + dT*(k1+2*k2+2*k3+k4)/6
for (i = 0; i < NUMX; i++)
X[i] =
Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
K4[i]) / 6;
Xlast[i] + dT * (K1[i] + 2.0f * K2[i] + 2.0f * K3[i] +
K4[i]) / 6.0f;
}
// ************* Model Specific Stuff ***************************
@ -674,23 +674,23 @@ void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
// Vdot = Reb*a
Xdot[3] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2.0f * (q1 * q2 -
q0 * q3) *
ay + 2 * (q1 * q3 + q0 * q2) * az;
ay + 2.0f * (q1 * q3 + q0 * q2) * az;
Xdot[4] =
2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2 * (q2 * q3 -
2.0f * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
q3 * q3) * ay + 2.0f * (q2 * q3 -
q0 * q1) *
az;
Xdot[5] =
2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
2.0f * (q1 * q3 - q0 * q2) * ax + 2.0f * (q2 * q3 + q0 * q1) * ay +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81f;
// qdot = Q*w
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2.0f;
Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2.0f;
Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2.0f;
Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2.0f;
// best guess is that bias stays constant
Xdot[10] = Xdot[11] = Xdot[12] = 0;
@ -715,58 +715,58 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
q3 = X[9];
// Pdot = V
F[0][3] = F[1][4] = F[2][5] = 1;
F[0][3] = F[1][4] = F[2][5] = 1.0f;
// dVdot/dq
F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az);
F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az);
F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az);
F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az);
F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az);
F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az);
F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az);
// dVdot/dabias & dVdot/dna
F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2.0f*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2.0f*(q1*q3+q0*q2);
F[4][13]=G[4][3]=-2.0f*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2.0f*(-q2*q3+q0*q1);
F[5][13]=G[5][3]=2.0f*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2.0f*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dq
F[6][6] = 0;
F[6][7] = -wx / 2;
F[6][8] = -wy / 2;
F[6][9] = -wz / 2;
F[7][6] = wx / 2;
F[6][7] = -wx / 2.0f;
F[6][8] = -wy / 2.0f;
F[6][9] = -wz / 2.0f;
F[7][6] = wx / 2.0f;
F[7][7] = 0;
F[7][8] = wz / 2;
F[7][9] = -wy / 2;
F[8][6] = wy / 2;
F[8][7] = -wz / 2;
F[7][8] = wz / 2.0f;
F[7][9] = -wy / 2.0f;
F[8][6] = wy / 2.0f;
F[8][7] = -wz / 2.0f;
F[8][8] = 0;
F[8][9] = wx / 2;
F[9][6] = wz / 2;
F[9][7] = wy / 2;
F[9][8] = -wx / 2;
F[8][9] = wx / 2.0f;
F[9][6] = wz / 2.0f;
F[9][7] = wy / 2.0f;
F[9][8] = -wx / 2.0f;
F[9][9] = 0;
// dqdot/dwbias
F[6][10] = q1 / 2;
F[6][11] = q2 / 2;
F[6][12] = q3 / 2;
F[7][10] = -q0 / 2;
F[7][11] = q3 / 2;
F[7][12] = -q2 / 2;
F[8][10] = -q3 / 2;
F[8][11] = -q0 / 2;
F[8][12] = q1 / 2;
F[9][10] = q2 / 2;
F[9][11] = -q1 / 2;
F[9][12] = -q0 / 2;
F[6][10] = q1 / 2.0f;
F[6][11] = q2 / 2.0f;
F[6][12] = q3 / 2.0f;
F[7][10] = -q0 / 2.0f;
F[7][11] = q3 / 2.0f;
F[7][12] = -q2 / 2.0f;
F[8][10] = -q3 / 2.0f;
F[8][11] = -q0 / 2.0f;
F[8][12] = q1 / 2.0f;
F[9][10] = q2 / 2.0f;
F[9][11] = -q1 / 2.0f;
F[9][12] = -q0 / 2.0f;
// dVdot/dna - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3); G[3][5]=-2*(q1*q3+q0*q2);
@ -774,23 +774,23 @@ void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
//G[5][3]=2*(-q1*q3+q0*q2); G[5][4]=-2*(q2*q3+q0*q1); G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
// dqdot/dnw
G[6][0] = q1 / 2;
G[6][1] = q2 / 2;
G[6][2] = q3 / 2;
G[7][0] = -q0 / 2;
G[7][1] = q3 / 2;
G[7][2] = -q2 / 2;
G[8][0] = -q3 / 2;
G[8][1] = -q0 / 2;
G[8][2] = q1 / 2;
G[9][0] = q2 / 2;
G[9][1] = -q1 / 2;
G[9][2] = -q0 / 2;
G[6][0] = q1 / 2.0f;
G[6][1] = q2 / 2.0f;
G[6][2] = q3 / 2.0f;
G[7][0] = -q0 / 2.0f;
G[7][1] = q3 / 2.0f;
G[7][2] = -q2 / 2.0f;
G[8][0] = -q3 / 2.0f;
G[8][1] = -q0 / 2.0f;
G[8][2] = q1 / 2.0f;
G[9][0] = q2 / 2.0f;
G[9][1] = -q1 / 2.0f;
G[9][2] = -q0 / 2.0f;
// dwbias = random walk noise
G[10][6] = G[11][7] = G[12][8] = 1;
G[10][6] = G[11][7] = G[12][8] = 1.0f;
// dabias = random walk noise
G[13][9] = G[14][10] = G[15][11] = 1;
G[13][9] = G[14][10] = G[15][11] = 1.0f;
}
void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
@ -813,19 +813,19 @@ void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
// Bb=Rbe*Be
Y[6] =
(q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
2.0f * (q1 * q2 + q0 * q3) * Be[1] + 2.0f * (q1 * q3 -
q0 * q2) * Be[2];
Y[7] =
2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
2.0f * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
q2 * q2 - q3 * q3) * Be[1] +
2 * (q2 * q3 + q0 * q1) * Be[2];
2.0f * (q2 * q3 + q0 * q1) * Be[2];
Y[8] =
2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
2.0f * (q1 * q3 + q0 * q2) * Be[0] + 2.0f * (q2 * q3 -
q0 * q1) * Be[1] +
(q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
// Alt = -Pz
Y[9] = -X[2];
Y[9] = X[2] * -1.0f;
}
void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
@ -838,26 +838,26 @@ void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
q3 = X[9];
// dP/dP=I;
H[0][0] = H[1][1] = H[2][2] = 1;
H[0][0] = H[1][1] = H[2][2] = 1.0f;
// dV/dV=I;
H[3][3] = H[4][4] = H[5][5] = 1;
H[3][3] = H[4][4] = H[5][5] = 1.0f;
// dBb/dq
H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][6] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[6][7] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[6][8] = 2.0f * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
H[6][9] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][6] = 2.0f * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
H[7][7] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[7][8] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
H[7][9] = 2.0f * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
H[8][6] = 2.0f * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
H[8][7] = 2.0f * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
H[8][8] = 2.0f * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
H[8][9] = 2.0f * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
// dAlt/dPz = -1
H[9][2] = -1;
H[9][2] = -1.0f;
}
/**