void JacobianBasis::getMetricMinAndGradients(const fullMatrix<double> &nodesXYZ,
const fullMatrix<double> &nodesXYZStraight,
fullVector<double> &lambdaJ, fullMatrix<double> &gradLambdaJ) const
{
// jacobian of the straight elements (only triangles for now)
SPoint3 v0(nodesXYZ(0,0),nodesXYZ(0,1),nodesXYZ(0,2));
SPoint3 v1(nodesXYZ(1,0),nodesXYZ(1,1),nodesXYZ(1,2));
SPoint3 v2(nodesXYZ(2,0),nodesXYZ(2,1),nodesXYZ(2,2));
SPoint3 *IXYZ[3] = {&v0, &v1, &v2};
double jaci[2][2] = {
{IXYZ[1]->x() - IXYZ[0]->x(), IXYZ[2]->x() - IXYZ[0]->x()},
{IXYZ[1]->y() - IXYZ[0]->y(), IXYZ[2]->y() - IXYZ[0]->y()}
};
double invJaci[2][2];
inv2x2(jaci, invJaci);
for (int l = 0; l < numJacNodes; l++) {
double jac[2][2] = {{0., 0.}, {0., 0.}};
for (int i = 0; i < numMapNodes; i++) {
const double &dPhidX = _gradBasis->gradShapeMatX(l,i);
const double &dPhidY = _gradBasis->gradShapeMatY(l,i);
const double dpsidx = dPhidX * invJaci[0][0] + dPhidY * invJaci[1][0];
const double dpsidy = dPhidX * invJaci[0][1] + dPhidY * invJaci[1][1];
jac[0][0] += nodesXYZ(i,0) * dpsidx;
jac[0][1] += nodesXYZ(i,0) * dpsidy;
jac[1][0] += nodesXYZ(i,1) * dpsidx;
jac[1][1] += nodesXYZ(i,1) * dpsidy;
}
const double dxdx = jac[0][0] * jac[0][0] + jac[0][1] * jac[0][1];
const double dydy = jac[1][0] * jac[1][0] + jac[1][1] * jac[1][1];
const double dxdy = jac[0][0] * jac[1][0] + jac[0][1] * jac[1][1];
const double sqr = sqrt((dxdx - dydy) * (dxdx - dydy) + 4 * dxdy * dxdy);
const double osqr = sqr > 1e-8 ? 1/sqr : 0;
lambdaJ(l) = 0.5 * (dxdx + dydy - sqr);
const double axx = (1 - (dxdx - dydy) * osqr) * jac[0][0] - 2 * dxdy * osqr * jac[1][0];
const double axy = (1 - (dxdx - dydy) * osqr) * jac[0][1] - 2 * dxdy * osqr * jac[1][1];
const double ayx = -2 * dxdy * osqr * jac[0][0] + (1 - (dydy - dxdx) * osqr) * jac[1][0];
const double ayy = -2 * dxdy * osqr * jac[0][1] + (1 - (dydy - dxdx) * osqr) * jac[1][1];
const double axixi = axx * invJaci[0][0] + axy * invJaci[0][1];
const double aetaeta = ayx * invJaci[1][0] + ayy * invJaci[1][1];
const double aetaxi = ayx * invJaci[0][0] + ayy * invJaci[0][1];
const double axieta = axx * invJaci[1][0] + axy * invJaci[1][1];
for (int i = 0; i < numMapNodes; i++) {
const double &dPhidX = _gradBasis->gradShapeMatX(l,i);
const double &dPhidY = _gradBasis->gradShapeMatY(l,i);
gradLambdaJ(l, i + 0 * numMapNodes) = axixi * dPhidX + axieta * dPhidY;
gradLambdaJ(l, i + 1 * numMapNodes) = aetaxi * dPhidX + aetaeta * dPhidY;
}
}
}
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;
}
