void
AP_DCM::normalize(void)
{
float error = 0;
DCM_Vector temporary[3];
uint8_t problem = 0;
error = -_dcm_matrix(0).dot_product(_dcm_matrix(1)) * 0.5; // eq.19
temporary[0] = _dcm_matrix(1) * error + _dcm_matrix(0); // eq.19
temporary[1] = _dcm_matrix(0) * error + _dcm_matrix(1); // eq.19
temporary[2] = temporary[0] ^ temporary[1]; // c= a x b // eq.20
_dcm_matrix(0) = _renorm(temporary[0], problem);
_dcm_matrix(1) = _renorm(temporary[1], problem);
_dcm_matrix(2) = _renorm(temporary[2], problem);
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
_dcm_matrix(0, 0)= 1.0f;
_dcm_matrix(0, 1)= 0.0f;
_dcm_matrix(0, 2)= 0.0f;
_dcm_matrix(1, 0)= 0.0f;
_dcm_matrix(1, 1)= 1.0f;
_dcm_matrix(1, 2)= 0.0f;
_dcm_matrix(2, 0)= 0.0f;
_dcm_matrix(2, 1)= 0.0f;
_dcm_matrix(2, 2)= 1.0f;
}
}
void
AP_DCM_FW::normalize(void)
{
float error = 0;
Vector3f temporary[3];
int problem = 0;
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
temporary[0] = _dcm_matrix.b;
temporary[1] = _dcm_matrix.a;
temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error)); // eq.19
temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error)); // eq.19
temporary[2] = temporary[0] % temporary[1]; // c= a x b // eq.20
_dcm_matrix.a = _renorm(temporary[0], problem);
_dcm_matrix.b = _renorm(temporary[1], problem);
_dcm_matrix.c = _renorm(temporary[2], problem);
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
_dcm_matrix.a.x = 1.0f;
_dcm_matrix.a.y = 0.0f;
_dcm_matrix.a.z = 0.0f;
_dcm_matrix.b.x = 0.0f;
_dcm_matrix.b.y = 1.0f;
_dcm_matrix.b.z = 0.0f;
_dcm_matrix.c.x = 0.0f;
_dcm_matrix.c.y = 0.0f;
_dcm_matrix.c.z = 1.0f;
Serial.println("Solution blew up");
/*
Serial.println(_dcm_matrix.a.x);
Serial.println(_dcm_matrix.a.y);
Serial.println(_dcm_matrix.a.z);
Serial.println(_dcm_matrix.b.x);
Serial.println(_dcm_matrix.b.y);
Serial.println(_dcm_matrix.b.z);
Serial.println(_dcm_matrix.c.x);
Serial.println(_dcm_matrix.c.y);
Serial.println(_dcm_matrix.c.z);
*/
}
}
