void kalman_roll_pitch_update(void)
{
float det;
//update covariance matrices
kalman_Q_R_init();
//time update
//get angle measurement
z[0]=estimator_kalman_theta;
z[1]=estimator_kalman_phi;
//x__=A*x_+B*x_dot
mult3x3by3x1(A,x_,tmp3_1);
mult3x2by2x1(B,x_dot,tmp3_2);
sum3x1and3x1(tmp3_1,tmp3_2,x__);
//P=A*P_last*A'+Q
transpose3x3(A,tmp9_2);
mult3x3by3x3(A,P_last,tmp9_1);
mult3x3by3x3(tmp9_1,tmp9_2,tmp9_3);
sum3x3and3x3(tmp9_3,Q,P);
//measurement update
//K=P*H'*inv(H*P*H'+R);
transpose2x3(H,tmp6_2);
mult2x3by3x3(H,P,tmp6_1);
mult2x3by3x2(tmp6_1,tmp6_2,tmp4_1);
sum2x2and2x2(tmp4_1,R,tmp4_2);
inv2x2(tmp4_2,tmp4_1,det);
mult3x3by3x2(P,tmp6_2,tmp6_1);
mult3x2by2x2(tmp6_1,tmp4_1,K);
//x_=x__+K*(z-H*x__);
mult2x3by3x1(H,x__,tmp2_1);
multconstby2x1(-1,tmp2_1);
sum2x1and2x1(z,tmp2_1,tmp2_2);
mult3x2by2x1(K,tmp2_2,tmp3_1);
sum3x1and3x1(x__,tmp3_1,x_);
//P_last=(eye(3)-K*H)*P;
diag3x3(1,1,1,tmp9_1);
mult3x2by2x3(K,H,tmp9_2);
multconstby3x3(-1,tmp9_2);
sum3x3and3x3(tmp9_1,tmp9_2,tmp9_3);
mult3x3by3x3(tmp9_3,P,P_last);
//assign output variables
estimator_kalman_theta=x_[0];
estimator_kalman_phi=x_[1];
//get angular velocities for the next iteration
x_dot[0]=estimator_q;
x_dot[1]=estimator_p;
}
void testQuaternionMultiply()
{
HEADER;
dMatrix3 RA,RB,RC,Rtest;
dQuaternion qa,qb,qc;
dReal diff,maxdiff=0;
for (int i=0; i<100; i++) {
makeRandomRotation (RB);
makeRandomRotation (RC);
dRtoQ (RB,qb);
dRtoQ (RC,qc);
dMultiply0 (RA,RB,RC,3,3,3);
dQMultiply0 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply1 (RA,RB,RC,3,3,3);
dQMultiply1 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply2 (RA,RB,RC,3,3,3);
dQMultiply2 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
dMultiply0 (RA,RC,RB,3,3,3);
transpose3x3 (RA);
dQMultiply3 (qa,qb,qc);
dQtoR (qa,Rtest);
diff = dMaxDifference (Rtest,RA,3,3);
if (diff > maxdiff) maxdiff = diff;
}
printf ("\tmaximum difference = %e - %s\n",maxdiff,
(maxdiff > tol) ? "FAILED" : "passed");
}
void kepler_orientation_matrix(double i, double an, double arg, double *mat) {
kepler_orientation_tangent(i, an, arg, mat+0);
kepler_orientation_bitangent(i, an, arg, mat+3);
kepler_orientation_normal(i, an, arg, mat+6);
transpose3x3(mat);
}