void Mat_scale ( ) {
uint32_t R1 = 0;
uint32_t C1 = 0;
uint32_t R2 = 0;
uint32_t C2 = 0;
uint32_t Scaling_factor = 0;
arm_status status;
arm_matrix_instance_f32 A ;
arm_matrix_instance_f32 Scale_A__OUT ;
const float32_t A_data[4];
const float32_t Scale_A__OUT_data[4];
arm_mat_init_f32( &A ,
R1 ,
C1,
A_data );
arm_mat_init_f32( &Scale_A__OUT ,
C2 ,
Scaling_factor ,
Scale_A__OUT_data );
status = arm_mat_scale_f32( &A ,
R2,
&Scale_A__OUT );
}
int32_t main()
{
#ifndef USE_STATIC_INIT
arm_matrix_instance_f32 srcA;
arm_matrix_instance_f32 srcB;
arm_matrix_instance_f32 dstC;
/* Input and output matrices initializations */
arm_mat_init_f32(&srcA, numStudents, numSubjects, (float32_t *)testMarks_f32);
arm_mat_init_f32(&srcB, numSubjects, 1, (float32_t *)testUnity_f32);
arm_mat_init_f32(&dstC, numStudents, 1, testOutput);
#else
/* Static Initializations of Input and output matrix sizes and array */
arm_matrix_instance_f32 srcA = {NUMSTUDENTS, NUMSUBJECTS, (float32_t *)testMarks_f32};
arm_matrix_instance_f32 srcB = {NUMSUBJECTS, 1, (float32_t *)testUnity_f32};
arm_matrix_instance_f32 dstC = {NUMSTUDENTS, 1, testOutput};
#endif
/* ----------------------------------------------------------------------
*Call the Matrix multiplication process function
* ------------------------------------------------------------------- */
arm_mat_mult_f32(&srcA, &srcB, &dstC);
/* ----------------------------------------------------------------------
** Call the Max function to calculate max marks among numStudents
** ------------------------------------------------------------------- */
arm_max_f32(testOutput, numStudents, &max_marks, &student_num);
/* ----------------------------------------------------------------------
** Call the Min function to calculate min marks among numStudents
** ------------------------------------------------------------------- */
arm_min_f32(testOutput, numStudents, &min_marks, &student_num);
/* ----------------------------------------------------------------------
** Call the Mean function to calculate mean
** ------------------------------------------------------------------- */
arm_mean_f32(testOutput, numStudents, &mean);
/* ----------------------------------------------------------------------
** Call the std function to calculate standard deviation
** ------------------------------------------------------------------- */
arm_std_f32(testOutput, numStudents, &std);
/* ----------------------------------------------------------------------
** Call the var function to calculate variance
** ------------------------------------------------------------------- */
arm_var_f32(testOutput, numStudents, &var);
while(1); /* main function does not return */
}
/** \brief Inicilização do controle de estabilidade.
*
* O controlador utiliza a API de DSP da CMSIS, e portanto se baseia fortemente no uso do
* tipo arm_matrix_instance_f32. Esta \b struct contêm os valores de número de linhas e
* colunas de matriz, além de um ponteiro para seus elementos (na forma de array).
* Estes arrays são prealocados globalmente (ver código fonte), para evitar overhead
* de alocação dinâmica em cada chamada e para evitar que, a cada alocação em uma função, a memória para
* a qual o ponteiro aponta saia de escopo e seja deletada. Uma vez que as funções são privadas e chamadas
* em ordem determinística, mutexes não são implementadas (por simplicidade apenas)
*/
void c_rc_LQR2_control_init() {
// Inicializa as matrizes estaticas
arm_mat_init_f32(&c_rc_LQR2_equilibrium_control, 4, 1, (float32_t *)c_rc_LQR2_equilibrium_control_f32);
#ifdef LQR_INTEGRATOR
arm_mat_init_f32(&c_rc_LQR2_Ki, 4, 18, (float32_t *)c_rc_LQR2_Ki_f32);
#else
arm_mat_init_f32(&c_rc_LQR2_Ke, 4, 16, (float32_t *)c_rc_LQR2_Ke_f32);
#endif
}
arm_matrix_instance_f32 c_rc_LQR2_errorStateVector(pv_type_datapr_attitude attitude,
pv_type_datapr_attitude attitude_reference,
pv_type_datapr_position position,
pv_type_datapr_position position_reference,
pv_type_datapr_servos servo,
pv_type_datapr_servos servo_reference){
arm_matrix_instance_f32 c_rc_LQR2_error_state_vector, c_rc_LQR2_state_vector, c_rc_LQR2_reference_state_vector;
//State Vector
c_rc_LQR2_state_vector_f32[STATE_X]=position.x;
c_rc_LQR2_state_vector_f32[STATE_Y]=position.y;
c_rc_LQR2_state_vector_f32[STATE_Z]=position.z;
c_rc_LQR2_state_vector_f32[STATE_ROLL]=attitude.roll;
c_rc_LQR2_state_vector_f32[STATE_PITCH]=attitude.pitch;
c_rc_LQR2_state_vector_f32[STATE_YAW]=attitude.yaw;
c_rc_LQR2_state_vector_f32[STATE_ALPHA_R]=servo.alphar;
c_rc_LQR2_state_vector_f32[STATE_ALPHA_L]=servo.alphal;
c_rc_LQR2_state_vector_f32[STATE_DX]=position.dotX;
c_rc_LQR2_state_vector_f32[STATE_DY]=position.dotY;
c_rc_LQR2_state_vector_f32[STATE_DZ]=position.dotZ;
c_rc_LQR2_state_vector_f32[STATE_DROLL]=attitude.dotRoll;
c_rc_LQR2_state_vector_f32[STATE_DPITCH]=attitude.dotPitch;
c_rc_LQR2_state_vector_f32[STATE_DYAW]=attitude.dotYaw;
c_rc_LQR2_state_vector_f32[STATE_DALPHA_R]=servo.dotAlphar;
c_rc_LQR2_state_vector_f32[STATE_DALPHA_L]=servo.dotAlphal;
//Updates the height equilibrium point according to the reference
c_rc_LQR2_state_vector_reference_f32[STATE_X]=position_reference.x;
c_rc_LQR2_state_vector_reference_f32[STATE_Y]=position_reference.y;
c_rc_LQR2_state_vector_reference_f32[STATE_Z]=position_reference.z;
c_rc_LQR2_state_vector_reference_f32[STATE_ROLL]=attitude_reference.roll;
c_rc_LQR2_state_vector_reference_f32[STATE_PITCH]=attitude_reference.pitch;
c_rc_LQR2_state_vector_reference_f32[STATE_YAW]=attitude_reference.yaw;
c_rc_LQR2_state_vector_reference_f32[STATE_ALPHA_R]=servo_reference.alphar;
c_rc_LQR2_state_vector_reference_f32[STATE_ALPHA_L]=servo_reference.alphal;
c_rc_LQR2_state_vector_reference_f32[STATE_DX]=position_reference.dotX;
c_rc_LQR2_state_vector_reference_f32[STATE_DY]=position_reference.dotY;
c_rc_LQR2_state_vector_reference_f32[STATE_DZ]=position_reference.dotZ;
c_rc_LQR2_state_vector_reference_f32[STATE_DROLL]=attitude_reference.dotRoll;
c_rc_LQR2_state_vector_reference_f32[STATE_DPITCH]=attitude_reference.dotPitch;
c_rc_LQR2_state_vector_reference_f32[STATE_DYAW]=attitude_reference.dotYaw;
c_rc_LQR2_state_vector_reference_f32[STATE_DALPHA_R]=servo_reference.dotAlphar;
c_rc_LQR2_state_vector_reference_f32[STATE_DALPHA_L]=servo_reference.dotAlphal;
//Initializes the matrices
arm_mat_init_f32(&c_rc_LQR2_state_vector, 16, 1, (float32_t *)c_rc_LQR2_state_vector_f32);
arm_mat_init_f32(&c_rc_LQR2_reference_state_vector, 16, 1, (float32_t *)c_rc_LQR2_state_vector_reference_f32);
arm_mat_init_f32(&c_rc_LQR2_error_state_vector, 16, 1, (float32_t *)c_rc_LQR2_error_state_vector_f32);
//e(t)=x(k)- xr(k)
arm_mat_sub_f32(&c_rc_LQR2_state_vector, &c_rc_LQR2_reference_state_vector, &c_rc_LQR2_error_state_vector);
return c_rc_LQR2_error_state_vector;
}
void matrixInit(arm_matrix_instance_f32 *m, int rows, int cols) {
float32_t *d;
d = (float32_t *)aqDataCalloc(rows*cols, sizeof(float32_t));
arm_mat_init_f32(m, rows, cols, d);
arm_fill_f32(0, d, rows*cols);
}
pv_type_actuation c_rc_LQR2_controller(pv_type_datapr_attitude attitude,
pv_type_datapr_attitude attitude_reference,
pv_type_datapr_position position,
pv_type_datapr_position position_reference,
pv_type_datapr_servos servo,
pv_type_datapr_servos servo_reference,
float sample_time, bool stop){
pv_type_actuation actuation_signals;
#ifdef LQR_INTEGRATOR
arm_matrix_instance_f32 c_rc_LQR2_error_state_vector, c_rc_LQR2_control_output, c_rc_LQR2_i_control_output;
//Initialize result matrices
arm_mat_init_f32(&c_rc_LQR2_control_output, 4, 1, (float32_t *)c_rc_LQR2_control_output_f32);
arm_mat_init_f32(&c_rc_LQR2_i_control_output, 4, 1, (float32_t *)c_rc_LQR2_i_control_output_f32);
//e(k)=xs_a(k)-xr_a(k)
c_rc_LQR2_error_state_vector = c_rc_LQR2_errorStateVector_I(attitude, attitude_reference, position, position_reference,servo, servo_reference,sample_time, stop);
//u=Ke*e(t)
arm_mat_mult_f32(&c_rc_LQR2_Ki, &c_rc_LQR2_error_state_vector, &c_rc_LQR2_i_control_output);
arm_mat_add_f32(&c_rc_LQR2_equilibrium_control, &c_rc_LQR2_i_control_output, &c_rc_LQR2_control_output);
#else
arm_matrix_instance_f32 c_rc_LQR2_error_state_vector, c_rc_LQR2_control_output, c_rc_LQR2_i_control_output;
//Initialize result matrices
arm_mat_init_f32(&c_rc_LQR2_control_output, 4, 1, (float32_t *)c_rc_LQR2_control_output_f32);
arm_mat_init_f32(&c_rc_LQR2_i_control_output, 4, 1, (float32_t *)c_rc_LQR2_i_control_output_f32);
//e(k)=xs_a(k)-xr_a(k)
c_rc_LQR2_error_state_vector = c_rc_LQR2_errorStateVector(attitude, attitude_reference, position, position_reference,servo, servo_reference);
//u=Ke*e(t)
arm_mat_mult_f32(&c_rc_LQR2_Ke, &c_rc_LQR2_error_state_vector, &c_rc_LQR2_i_control_output);
arm_mat_add_f32(&c_rc_LQR2_equilibrium_control, &c_rc_LQR2_i_control_output, &c_rc_LQR2_control_output);
#endif
//The result must be in a struct pv_msg_io_actuation
actuation_signals.escRightNewtons=(float)c_rc_LQR2_control_output.pData[0]/1000;
actuation_signals.escLeftNewtons=(float)c_rc_LQR2_control_output.pData[1]/1000;
actuation_signals.servoRight=(float)c_rc_LQR2_control_output.pData[2]/1000;
actuation_signals.servoLeft=(float)c_rc_LQR2_control_output.pData[3]/1000;
actuation_signals.escRightSpeed=0;
actuation_signals.escLeftSpeed=0;
return actuation_signals;
}
void Matrix_subtraction ( ) {
uint32_t r1 = 0;
uint32_t c1 = 0;
uint32_t cout = 0;
uint32_t rout = 0;
uint32_t c2 = 0;
uint32_t r2 = 0;
arm_status status;
arm_matrix_instance_f32 A ;
arm_matrix_instance_f32 B ;
arm_matrix_instance_f32 A_SUB_B_OUT ;
const float32_t A_data[4];
const float32_t B_data[4];
const float32_t A_SUB_B_OUT_data[4];
arm_mat_init_f32( &A ,
r1 ,
c1,
A_data );
arm_mat_init_f32( &B ,
r2 ,
c2,
B_data );
arm_mat_init_f32( &A_SUB_B_OUT ,
rout ,
cout ,
A_SUB_B_OUT_data );
status = arm_mat_sub_f32( &A ,
&B , &A_SUB_B_OUT );
}
void Rotate3dVector(float vector[3], float roll, float pitch, float yaw, float Retorno[3])
{
roll = roll/57.3;
pitch = pitch/57.3;
yaw = yaw/57.3;
float A = roll;
float B = pitch;
float C = yaw;
float cosA, sinA;
float cosB, sinB;
float cosC, sinC;
cosA = arm_cos_f32(A);
sinA = arm_sin_f32(A);
cosB = arm_cos_f32(B);
sinB = arm_sin_f32(B);
cosC = arm_cos_f32(C);
sinC = arm_sin_f32(C);
float RotationMatrix_f32[9] = {cosB*cosC, cosC*sinA*sinB-cosA*sinC, cosA*cosC*sinB+sinA*sinC,
cosB*sinC, cosA*cosC+sinA*sinB*sinC, -cosC*sinA+cosA*sinB*sinC,
-sinB, cosB*cosA, cosA*cosB};
arm_matrix_instance_f32 RotationMatrix;
arm_mat_init_f32(&RotationMatrix, 3, 3, RotationMatrix_f32);
arm_matrix_instance_f32 InVector;
arm_mat_init_f32(&InVector, 3, 1, vector);
arm_matrix_instance_f32 OutVector;
arm_mat_init_f32(&OutVector, 3, 1, Retorno);
arm_mat_mult_f32(&RotationMatrix, &InVector, &OutVector);
}
int main(void)
{
///////////////////////////////////////////////////////////////////////////
uint32_t currentTime;
arm_matrix_instance_f32 a;
arm_matrix_instance_f32 b;
arm_matrix_instance_f32 x;
systemReady = false;
systemInit();
systemReady = true;
evrPush(EVR_StartingMain, 0);
#ifdef _DTIMING
#define LA1_ENABLE GPIO_SetBits(GPIOA, GPIO_Pin_4)
#define LA1_DISABLE GPIO_ResetBits(GPIOA, GPIO_Pin_4)
#define LA4_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_5)
#define LA4_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_5)
#define LA2_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_2)
#define LA2_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_2)
#define LA3_ENABLE GPIO_SetBits(GPIOC, GPIO_Pin_3)
#define LA3_DISABLE GPIO_ResetBits(GPIOC, GPIO_Pin_3)
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
GPIO_StructInit(&GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOB, &GPIO_InitStructure);
// Init pins
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5;
//GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
//GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
//GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
//GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// PB0_DISABLE;
LA4_DISABLE;
LA2_DISABLE;
LA3_DISABLE;
LA1_DISABLE;
#endif
while (1)
{
checkUsbActive(false);
evrCheck();
///////////////////////////////
if (frame_50Hz)
{
#ifdef _DTIMING
LA2_ENABLE;
#endif
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
if (newTemperatureReading && newPressureReading)
{
d1Value = d1.value;
d2Value = d2.value;
calculateTemperature();
calculatePressureAltitude();
newTemperatureReading = false;
newPressureReading = false;
}
sensors.pressureAlt50Hz = firstOrderFilter(sensors.pressureAlt50Hz, &firstOrderFilters[PRESSURE_ALT_LOWPASS]);
rssiMeasure();
updateMax7456(currentTime, 0);
executionTime50Hz = micros() - currentTime;
#ifdef _DTIMING
LA2_DISABLE;
#endif
}
///////////////////////////////
if (frame_10Hz)
{
#ifdef _DTIMING
LA4_ENABLE;
#endif
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
if (newMagData == true)
{
nonRotatedMagData[XAXIS] = (rawMag[XAXIS].value * magScaleFactor[XAXIS]) - eepromConfig.magBias[XAXIS + eepromConfig.externalHMC5883];
nonRotatedMagData[YAXIS] = (rawMag[YAXIS].value * magScaleFactor[YAXIS]) - eepromConfig.magBias[YAXIS + eepromConfig.externalHMC5883];
nonRotatedMagData[ZAXIS] = (rawMag[ZAXIS].value * magScaleFactor[ZAXIS]) - eepromConfig.magBias[ZAXIS + eepromConfig.externalHMC5883];
arm_mat_init_f32(&a, 3, 3, (float *)hmcOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedMagData);
arm_mat_init_f32(&x, 3, 1, sensors.mag10Hz);
arm_mat_mult_f32(&a, &b, &x);
newMagData = false;
magDataUpdate = true;
}
decodeUbloxMsg();
batMonTick();
cliCom();
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendAttitude();
mavlinkSendVfrHud();
}
executionTime10Hz = micros() - currentTime;
#ifdef _DTIMING
LA4_DISABLE;
#endif
}
///////////////////////////////
if (frame_500Hz)
{
#ifdef _DTIMING
LA1_ENABLE;
#endif
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
TIM_Cmd(TIM10, DISABLE);
timerValue = TIM_GetCounter(TIM10);
TIM_SetCounter(TIM10, 0);
TIM_Cmd(TIM10, ENABLE);
dt500Hz = (float)timerValue * 0.0000005f; // For integrations in 500 Hz loop
computeMPU6000TCBias();
nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples500Hz[XAXIS] * 0.5f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples500Hz[YAXIS] * 0.5f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples500Hz[ZAXIS] * 0.5f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData);
arm_mat_init_f32(&x, 3, 1, sensors.accel500Hz);
arm_mat_mult_f32(&a, &b, &x);
nonRotatedGyroData[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] * 0.5f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
nonRotatedGyroData[PITCH] = ((float)gyroSummedSamples500Hz[PITCH] * 0.5f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
nonRotatedGyroData[YAW ] = ((float)gyroSummedSamples500Hz[YAW] * 0.5f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedGyroData);
arm_mat_init_f32(&x, 3, 1, sensors.gyro500Hz);
arm_mat_mult_f32(&a, &b, &x);
MargAHRSupdate(sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
sensors.accel500Hz[XAXIS], sensors.accel500Hz[YAXIS], sensors.accel500Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
eepromConfig.accelCutoff,
magDataUpdate,
dt500Hz);
magDataUpdate = false;
computeAxisCommands(dt500Hz);
mixTable();
writeServos();
writeMotors();
executionTime500Hz = micros() - currentTime;
#ifdef _DTIMING
LA1_DISABLE;
#endif
}
///////////////////////////////
if (frame_100Hz)
{
#ifdef _DTIMING
LA3_ENABLE;
#endif
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
TIM_Cmd(TIM11, DISABLE);
timerValue = TIM_GetCounter(TIM11);
TIM_SetCounter(TIM11, 0);
TIM_Cmd(TIM11, ENABLE);
dt100Hz = (float)timerValue * 0.0000005f; // For integrations in 100 Hz loop
nonRotatedAccelData[XAXIS] = ((float)accelSummedSamples100Hz[XAXIS] * 0.1f - accelTCBias[XAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[YAXIS] = ((float)accelSummedSamples100Hz[YAXIS] * 0.1f - accelTCBias[YAXIS]) * ACCEL_SCALE_FACTOR;
nonRotatedAccelData[ZAXIS] = ((float)accelSummedSamples100Hz[ZAXIS] * 0.1f - accelTCBias[ZAXIS]) * ACCEL_SCALE_FACTOR;
arm_mat_init_f32(&a, 3, 3, (float *)mpuOrientationMatrix);
arm_mat_init_f32(&b, 3, 1, (float *)nonRotatedAccelData);
arm_mat_init_f32(&x, 3, 1, sensors.accel100Hz);
arm_mat_mult_f32(&a, &b, &x);
createRotationMatrix();
bodyAccelToEarthAccel();
vertCompFilter(dt100Hz);
if (armed == true)
{
if ( eepromConfig.activeTelemetry == 1 )
{
// Roll Loop
openLogPortPrintF("1,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode,
rateCmd[ROLL],
sensors.gyro500Hz[ROLL],
ratePID[ROLL],
attCmd[ROLL],
sensors.attitude500Hz[ROLL],
attPID[ROLL]);
}
if ( eepromConfig.activeTelemetry == 2 )
{
// Pitch Loop
openLogPortPrintF("2,%1d,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f,%9.4f\n", flightMode,
rateCmd[PITCH],
sensors.gyro500Hz[PITCH],
ratePID[PITCH],
attCmd[PITCH],
sensors.attitude500Hz[PITCH],
attPID[PITCH]);
}
if ( eepromConfig.activeTelemetry == 4 )
{
// Sensors
openLogPortPrintF("3,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,%8.4f,\n", sensors.accel500Hz[XAXIS],
sensors.accel500Hz[YAXIS],
sensors.accel500Hz[ZAXIS],
sensors.gyro500Hz[ROLL],
sensors.gyro500Hz[PITCH],
sensors.gyro500Hz[YAW],
sensors.mag10Hz[XAXIS],
sensors.mag10Hz[YAXIS],
sensors.mag10Hz[ZAXIS],
sensors.attitude500Hz[ROLL],
sensors.attitude500Hz[PITCH],
sensors.attitude500Hz[YAW]);
}
if ( eepromConfig.activeTelemetry == 8 )
{
}
if ( eepromConfig.activeTelemetry == 16)
{
// Vertical Variables
openLogPortPrintF("%9.4f, %9.4f, %9.4f, %4ld, %1d, %9.4f\n", verticalVelocityCmd,
hDotEstimate,
hEstimate,
ms5611Temperature,
verticalModeState,
throttleCmd);
}
}
executionTime100Hz = micros() - currentTime;
#ifdef _DTIMING
LA3_DISABLE;
#endif
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
if (gpsValid() == true)
{
}
//if (eepromConfig.mavlinkEnabled == true)
//{
// mavlinkSendGpsRaw();
//}
if (batMonVeryLowWarning > 0)
{
LED1_TOGGLE;
batMonVeryLowWarning--;
}
if (execUp == true)
BLUE_LED_TOGGLE;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
if (execUp == true)
GREEN_LED_TOGGLE;
if (execUp == false)
execUpCount++;
// Initialize sensors after being warmed up
if ((execUpCount == 20) && (execUp == false))
{
computeMPU6000RTData();
initMag();
initPressure();
}
// Initialize PWM and set mag after sensor warmup
if ((execUpCount == 25) && (execUp == false))
{
execUp = true;
pwmEscInit();
homeData.magHeading = sensors.attitude500Hz[YAW];
}
if (batMonLowWarning > 0)
{
LED1_TOGGLE;
batMonLowWarning--;
}
if (eepromConfig.mavlinkEnabled == true)
{
mavlinkSendHeartbeat();
mavlinkSendSysStatus();
}
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
///////////////////////////////////////////////////////////////////////////
}
int32_t main(void)
{
arm_matrix_instance_f32 A; /* Matrix A Instance */
arm_matrix_instance_f32 AT; /* Matrix AT(A transpose) instance */
arm_matrix_instance_f32 ATMA; /* Matrix ATMA( AT multiply with A) instance */
arm_matrix_instance_f32 ATMAI; /* Matrix ATMAI(Inverse of ATMA) instance */
arm_matrix_instance_f32 B; /* Matrix B instance */
arm_matrix_instance_f32 X; /* Matrix X(Unknown Matrix) instance */
uint32_t srcRows, srcColumns; /* Temporary variables */
arm_status status;
/* Initialise A Matrix Instance with numRows, numCols and data array(A_f32) */
srcRows = 4;
srcColumns = 4;
arm_mat_init_f32(&A, srcRows, srcColumns, (float32_t *)A_f32);
/* Initialise Matrix Instance AT with numRows, numCols and data array(AT_f32) */
srcRows = 4;
srcColumns = 4;
arm_mat_init_f32(&AT, srcRows, srcColumns, AT_f32);
/* calculation of A transpose */
status = arm_mat_trans_f32(&A, &AT);
/* Initialise ATMA Matrix Instance with numRows, numCols and data array(ATMA_f32) */
srcRows = 4;
srcColumns = 4;
arm_mat_init_f32(&ATMA, srcRows, srcColumns, ATMA_f32);
/* calculation of AT Multiply with A */
status = arm_mat_mult_f32(&AT, &A, &ATMA);
/* Initialise ATMAI Matrix Instance with numRows, numCols and data array(ATMAI_f32) */
srcRows = 4;
srcColumns = 4;
arm_mat_init_f32(&ATMAI, srcRows, srcColumns, ATMAI_f32);
/* calculation of Inverse((Transpose(A) * A) */
status = arm_mat_inverse_f32(&ATMA, &ATMAI);
/* calculation of (Inverse((Transpose(A) * A)) * Transpose(A)) */
status = arm_mat_mult_f32(&ATMAI, &AT, &ATMA);
/* Initialise B Matrix Instance with numRows, numCols and data array(B_f32) */
srcRows = 4;
srcColumns = 1;
arm_mat_init_f32(&B, srcRows, srcColumns, (float32_t *)B_f32);
/* Initialise X Matrix Instance with numRows, numCols and data array(X_f32) */
srcRows = 4;
srcColumns = 1;
arm_mat_init_f32(&X, srcRows, srcColumns, X_f32);
/* calculation ((Inverse((Transpose(A) * A)) * Transpose(A)) * B) */
status = arm_mat_mult_f32(&ATMA, &B, &X);
/* Comparison of reference with test output */
snr = arm_snr_f32((float32_t *)xRef_f32, X_f32, 4);
/*------------------------------------------------------------------------------
* Initialise status depending on SNR calculations
*------------------------------------------------------------------------------*/
if( snr > SNR_THRESHOLD)
{
status = ARM_MATH_SUCCESS;
}
else
{
status = ARM_MATH_TEST_FAILURE;
}
/* ----------------------------------------------------------------------
** Loop here if the signals fail the PASS check.
** This denotes a test failure
** ------------------------------------------------------------------- */
if( status != ARM_MATH_SUCCESS)
{
while(1);
}
while(1); /* main function does not return */
}
void UKF_New(UKF_Filter* ukf)
{
float32_t *P = ukf->P_f32;
float32_t *Q = ukf->Q_f32;
float32_t *R = ukf->R_f32;
float32_t *Wc = ukf->Wc_f32;
//scaling factor
float32_t lambda = UKF_alpha * UKF_alpha *((float32_t)UKF_STATE_DIM + UKF_kappa) - (float32_t)UKF_STATE_DIM;
float32_t gamma = (float32_t)UKF_STATE_DIM + lambda;
//weights for means
ukf->Wm0 = lambda / gamma;
ukf->Wmi = 0.5f / gamma;
//weights for covariance
arm_mat_init_f32(&ukf->Wc, UKF_SP_POINTS, UKF_SP_POINTS, ukf->Wc_f32);
arm_mat_identity_f32(&ukf->Wc, ukf->Wmi);
Wc[0] = ukf->Wm0 + (1.0f - UKF_alpha * UKF_alpha + UKF_beta);
//scaling factor need to tune!
arm_sqrt_f32(gamma, &ukf->gamma);
//////////////////////////////////////////////////////////////////////////
//initialise P
arm_mat_init_f32(&ukf->P, UKF_STATE_DIM, UKF_STATE_DIM, ukf->P_f32);
arm_mat_identity_f32(&ukf->P, 1.0f);
P[0] = P[8] = P[16] = P[24] = UKF_PQ_INITIAL;
P[32] = P[40] = P[48] = UKF_PW_INITIAL;
arm_mat_init_f32(&ukf->PX, UKF_STATE_DIM, UKF_STATE_DIM, ukf->PX_f32);
arm_mat_init_f32(&ukf->PY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->PY_f32);
arm_mat_init_f32(&ukf->tmpPY, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->tmpPY_f32);
arm_mat_init_f32(&ukf->PXY, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->PXY_f32);
arm_mat_init_f32(&ukf->PXYT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->PXYT_f32);
//initialise Q
arm_mat_init_f32(&ukf->Q, UKF_STATE_DIM, UKF_STATE_DIM, ukf->Q_f32);
Q[0] = Q[8] = Q[16] = Q[24] = UKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = UKF_QW_INITIAL;
//initialise R
arm_mat_init_f32(&ukf->R, UKF_MEASUREMENT_DIM, UKF_MEASUREMENT_DIM, ukf->R_f32);
R[0] = R[14] = R[28] = R[42] = UKF_RQ_INITIAL;
R[56] = R[70] = R[84] = UKF_RA_INITIAL;
R[98] = R[112] = R[126] = UKF_RW_INITIAL;
R[140] = R[154] = R[168] = UKF_RM_INITIAL;
arm_mat_init_f32(&ukf->XSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->XSP_f32);
arm_mat_init_f32(&ukf->tmpXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpXSP_f32);
arm_mat_init_f32(&ukf->tmpXSPT, UKF_SP_POINTS, UKF_STATE_DIM, ukf->tmpXSPT_f32);
arm_mat_init_f32(&ukf->tmpWcXSP, UKF_STATE_DIM, UKF_SP_POINTS, ukf->tmpWcXSP_f32);
arm_mat_init_f32(&ukf->tmpWcYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpWcYSP_f32);
arm_mat_init_f32(&ukf->YSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->YSP_f32);
arm_mat_init_f32(&ukf->tmpYSP, UKF_MEASUREMENT_DIM, UKF_SP_POINTS, ukf->tmpYSP_f32);
arm_mat_init_f32(&ukf->tmpYSPT, UKF_SP_POINTS, UKF_MEASUREMENT_DIM, ukf->tmpYSPT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->K, UKF_STATE_DIM, UKF_MEASUREMENT_DIM, ukf->K_f32);
arm_mat_init_f32(&ukf->KT, UKF_MEASUREMENT_DIM, UKF_STATE_DIM, ukf->KT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->X, UKF_STATE_DIM, 1, ukf->X_f32);
arm_mat_zero_f32(&ukf->X);
arm_mat_init_f32(&ukf->tmpX, UKF_STATE_DIM, 1, ukf->tmpX_f32);
arm_mat_zero_f32(&ukf->tmpX);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ukf->Y, UKF_MEASUREMENT_DIM, 1, ukf->Y_f32);
arm_mat_zero_f32(&ukf->Y);
arm_mat_init_f32(&ukf->tmpY, UKF_MEASUREMENT_DIM, 1, ukf->tmpY_f32);
arm_mat_zero_f32(&ukf->tmpY);
//////////////////////////////////////////////////////////////////////////
}
void kalman_filter(kalman_filter_state *buffer_filtro, float medida_gyro[], float medida_accel[], float medida_mag[], float angles[], uint16_t estado_motores)
{
GPIO_ResetBits(GPIOD, GPIO_Pin_14);
//Insf_tancias das matrizes utilizadas para o cálculo
arm_matrix_instance_f32 X; //Matriz de estados. [n,1]
arm_matrix_instance_f32 F; //Matriz de transição de estados. [n,n]
arm_matrix_instance_f32 Ft; //Matriz de transição de estados transposta. [n,n]
arm_matrix_instance_f32 I; //Matriz identidadee. [n,n]
arm_matrix_instance_f32 P; //Matriz de confiabilidade do processo de atualização. [n,n]
//arm_matrix_instance_f32 h; //Matriz de mapeamento de observabilidade [a,n]
arm_matrix_instance_f32 H; //Matriz Jacobiana para atualização da confiabilidade do erro [a, n].
arm_matrix_instance_f32 Ht; //Matriz Jacobiana transposta para atualização da confiabilidade do erro. [n, a]
arm_matrix_instance_f32 Q; //Matriz de covariância multiplicada de processos; [n, n]
arm_matrix_instance_f32 R; //Matriz de variância [a ,a]
arm_matrix_instance_f32 S; //Matriz .... [a, a]
arm_matrix_instance_f32 Sinv; //Matriz S inversa. [a, a]
arm_matrix_instance_f32 K; //Matriz com os ganhos de Kalman [n,a]
//Matrices intermediàrias para cálculo
arm_matrix_instance_f32 temp_calc_a1_0;
arm_matrix_instance_f32 temp_calc_a1_1;
arm_matrix_instance_f32 temp_calc_n1_0;
arm_matrix_instance_f32 temp_calc_n1_1;
arm_matrix_instance_f32 temp_calc_aa_0;
arm_matrix_instance_f32 temp_calc_aa_1;
arm_matrix_instance_f32 temp_calc_na_0;
arm_matrix_instance_f32 temp_calc_an_0;
arm_matrix_instance_f32 temp_calc_nn_0;
arm_matrix_instance_f32 temp_calc_nn_1;
//Matriz S...
float S_f32[a*a];
arm_mat_init_f32(&S, a, a, S_f32);
float Sinv_f32[a*a];
arm_mat_init_f32(&Sinv, a, a, Sinv_f32);
//Matriz do ganho de Kalman
float K_f32[n*a];
arm_mat_init_f32(&K, n, a, K_f32);
//Matrizes de suporte para o cálculo
//Matrizes de 9 linhas e 1 coluna
float temp_calc_n1_0_f32[n];
float temp_calc_n1_1_f32[n];
arm_mat_init_f32(&temp_calc_n1_0, n, 1, temp_calc_n1_0_f32);
arm_mat_init_f32(&temp_calc_n1_1, n, 1, temp_calc_n1_1_f32);
//Matrizes de 9 linhas e 1 coluna
float temp_calc_a1_0_f32[a];
float temp_calc_a1_1_f32[a];
arm_mat_init_f32(&temp_calc_a1_0, a, 1, temp_calc_a1_0_f32);
arm_mat_init_f32(&temp_calc_a1_1, a, 1, temp_calc_a1_1_f32);
//Matrizes de 6 linhas e 6 colunas
float temp_calc_aa_0_f32[a*a];
float temp_calc_aa_1_f32[a*a];
arm_mat_init_f32(&temp_calc_aa_0, a, a, temp_calc_aa_0_f32);
arm_mat_init_f32(&temp_calc_aa_1, a, a, temp_calc_aa_1_f32);
//Matrizes de 9 linhas e 6 colunas
float temp_calc_na_0_f32[n*a];
arm_mat_init_f32(&temp_calc_na_0, n, a, temp_calc_na_0_f32);
//Matrizes de 6 linhas e 9 colunas
float temp_calc_an_0_f32[a*n];
arm_mat_init_f32(&temp_calc_an_0, a, n, temp_calc_an_0_f32);
//Matrizes de 9 linhas e 9 colunas
float temp_calc_nn_0_f32[n*n];
float temp_calc_nn_1_f32[n*n];
arm_mat_init_f32(&temp_calc_nn_0, n, n, temp_calc_nn_0_f32);
arm_mat_init_f32(&temp_calc_nn_1, n, n, temp_calc_nn_1_f32);
/*************************************Atualização dos dados para cálcuo*******************************/
//Variáveis para cálculos
float dt = buffer_filtro->dt;
/*Velocidades angulares subtraídas dos bias. */
float p = medida_gyro[0];
float q = medida_gyro[1];
float r = medida_gyro[2];
/*Atualização dos estados dos ângulos com base nas velocidades angulares e do bias estimado anteriormente*/
float X_f32[n];
arm_mat_init_f32(&X, n, 1, X_f32);
arm_copy_f32(buffer_filtro->ultimo_estado, X_f32, n);
float phi = X_f32[0];
float theta = X_f32[1];
float psi = X_f32[2];
float bPhi = X_f32[9];
float bTheta = X_f32[10];
float bPsi = X_f32[11];
//Atualizar o estado anterior - Apenas os ângulos precisam serm inicializados, uma vez que os bias são atualizados por uma identidade.
X_f32[0] = phi + (p)*dt + f_sin(phi)*f_tan(theta)*(q)*dt + f_cos(phi)*f_tan(theta)*(r)*dt;
X_f32[1] = theta + f_cos(phi)*(q)*dt - f_sin(phi)*(r)*dt;
X_f32[2] = psi + f_sin(phi)*f_sec(theta)*(q)*dt + f_cos(phi)*f_sec(theta)*(r)*dt;
phi = X_f32[0];
theta = X_f32[1];
psi = X_f32[2];
//Com os angulos calculados, cálcula os senos e cossenos utilizados para agilizar os processos de cálculo.
float cos_Phi = f_cos(phi);
float sin_Phi = f_sin(phi);
float cos_Theta = f_cos(theta);
float sin_Theta = f_sin(theta);
float tan_Theta = f_tan(theta);
//Elementos da matriz para atualização dos estados (F).
float F_f32[n*n] = { dt*q*cos_Phi*tan_Theta - dt*r*sin_Phi*tan_Theta + 1, dt*r*cos_Phi*(pow(tan_Theta,2) + 1) + dt*q*sin_Phi*(pow(tan_Theta,2) + 1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- dt*r*cos_Phi - dt*q*sin_Phi, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
(dt*q*cos_Phi)/cos_Theta - (dt*r*sin_Phi)/cos_Theta, (dt*r*cos_Phi*sin_Theta)/pow(cos_Theta,2) + (dt*q*sin_Phi*sin_Theta)/pow(cos_Theta,2), 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
arm_mat_init_f32(&F, n, n, F_f32);
//Matriz Jacobiana transposta para atualização de P.
float Ft_f32[n*n];
arm_mat_init_f32(&Ft, n, n, Ft_f32);
arm_mat_trans_f32(&F, &Ft);
//Processo à priori para atualização da matriz de confiabilidade P.
//Matriz de covariâncias do processo de atualização (Q).
float qAngles = (buffer_filtro->Q_angles);
float qBiasAcel = (buffer_filtro->Q_bias_acel);
float qBiasMag = (buffer_filtro->Q_bias_mag);
float qBiasAngles = (buffer_filtro->Q_bias_angle);
/*Matriz de confiabilidade do processo de atualização. */
/* Pk|k-1 = Pk-1|k-1 */
float P_f32[n*n];
arm_copy_f32(buffer_filtro->P, P_f32, n*n);
arm_mat_init_f32(&P, n, n, P_f32);
//temp_calc_nn_0 = F*P
if(arm_mat_mult_f32(&F, &P, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
while(1);
//temp_calc_nn_1 = F*P*F'
if(arm_mat_mult_f32(&temp_calc_nn_0, &Ft, &temp_calc_nn_1) != ARM_MATH_SUCCESS)
while(1);
//Atualiza a matriz de covariâncias dos processos
float Q_f32[n*n] = { qAngles, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, qAngles, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, qAngles, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, qBiasAcel, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, qBiasAcel, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, qBiasAcel, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, qBiasMag, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, qBiasMag, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, qBiasMag, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, qBiasAngles, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, qBiasAngles, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, qBiasAngles};
arm_mat_init_f32(&Q, n, n, Q_f32);
//P = temp_calc_nn_1 + Q = F*P*F' + Q
if(arm_mat_add_f32(&temp_calc_nn_1, &Q, &P) != ARM_MATH_SUCCESS)
while(1);
/*Estados iniciais do magnetômetro */
float magRefVector[3];
float acelRefVector[3];
arm_matrix_instance_f32 magRefMatrix;
arm_matrix_instance_f32 acelRefMatrix;
arm_mat_init_f32(&magRefMatrix, 3, 1, magRefVector);
arm_mat_init_f32(&acelRefMatrix, 3, 1, acelRefVector);
float ax = buffer_filtro->AcelInicial[0];
float ay = buffer_filtro->AcelInicial[1];
float az = buffer_filtro->AcelInicial[2];
float hx = buffer_filtro->MagInicial[0];
float hy = buffer_filtro->MagInicial[1];
float hz = buffer_filtro->MagInicial[2];
magRefVector[0] = hx;
magRefVector[1] = hy;
magRefVector[2] = hz;
acelRefVector[0] = ax;
acelRefVector[1] = ay;
acelRefVector[2] = az;
//Matrizes com o resultado das operações de rotação
float observatedStateVector[a];
arm_matrix_instance_f32 observatedStateMatrix;
arm_mat_init_f32(&observatedStateMatrix, a, 1, observatedStateVector);
//Matriz contendo os valores observados utilizados pra gerar o rezíduo (yk)
arm_matrix_instance_f32 rotatedMagMatrix;
arm_matrix_instance_f32 rotatedAcelMatrix;
arm_mat_init_f32(&rotatedAcelMatrix, 3, 1, observatedStateVector);
arm_mat_init_f32(&rotatedMagMatrix, 3, 1, observatedStateVector+3);
//Matriz de rotação com base nos angulos estimados.
float rotationVector[9];
arm_matrix_instance_f32 rotationMatrix;
arm_mat_init_f32(&rotationMatrix, 3, 3, rotationVector);
getABCRotMatFromEulerAngles(phi-bPhi, theta-bTheta, psi-bPsi, &rotationMatrix);
/* Cálculo das referências com base no magnetômetro e no estado do acelerômetro parado [0; 0; 1]; */
if(arm_mat_mult_f32(&rotationMatrix, &acelRefMatrix, &rotatedAcelMatrix) != ARM_MATH_SUCCESS)
while(1);
if(arm_mat_mult_f32(&rotationMatrix, &magRefMatrix, &rotatedMagMatrix) != ARM_MATH_SUCCESS)
while(1);
//Vetor com as médidas
float zkVector[a];
zkVector[0] = medida_accel[0];
zkVector[1] = medida_accel[1];
zkVector[2] = medida_accel[2];
zkVector[3] = medida_mag[0];
zkVector[4] = medida_mag[1];
zkVector[5] = medida_mag[2];
zkVector[6] = angles[0];
zkVector[7] = angles[1];
zkVector[8] = angles[2];
arm_matrix_instance_f32 zkMatrix;
arm_mat_init_f32(&zkMatrix, a, 1, zkVector);
//Vetor de resíduo
float ykVector[a];
arm_matrix_instance_f32 ykMatrix;
arm_mat_init_f32(&ykMatrix, a, 1, ykVector);
//Adiciona os bias estimados pelo filtro
observatedStateVector[0] += X_f32[3];
observatedStateVector[1] += X_f32[4];
observatedStateVector[2] += X_f32[5];
observatedStateVector[3] += X_f32[6];
observatedStateVector[4] += X_f32[7];
observatedStateVector[5] += X_f32[8];
//Remove o bias estimado pelo filtro
float correctedPhi = -(X_f32[0] - X_f32[9]);
float correctedTheta = -(X_f32[1] - X_f32[10]);
float correctedPsi = -(X_f32[2] - X_f32[11]);
//Com os angulos corrigidos calculados, cálcula os senos e cossenos utilizados para agilizar os processos de cálculo.
float cos_correctedPhi = f_cos(correctedPhi);
float sin_correctedPhi = f_sin(correctedPhi);
float cos_correctedTheta = f_cos(correctedTheta);
float sin_correctedTheta = f_sin(correctedTheta);
float cos_correctedPsi = f_cos(correctedPsi);
float sin_correctedPsi = f_sin(correctedPsi);
//
observatedStateVector[6] = -correctedPhi;
observatedStateVector[7] = -correctedTheta;
observatedStateVector[8] = -correctedPsi;
if(arm_mat_sub_f32(&zkMatrix, &observatedStateMatrix, &ykMatrix) != ARM_MATH_SUCCESS)
while(1);
float H_f32[a*n] = { 0, ax*sin_correctedTheta*cos_correctedPsi - az*cos_correctedTheta - ay*sin_correctedPsi*sin_correctedTheta, ay*cos_correctedPsi*cos_correctedTheta + ax*sin_correctedPsi*cos_correctedTheta, 1, 0, 0, 0, 0, 0, 0, az*cos_correctedTheta - ax*sin_correctedTheta*cos_correctedPsi + ay*sin_correctedPsi*sin_correctedTheta, - ay*cos_correctedPsi*cos_correctedTheta - ax*sin_correctedPsi*cos_correctedTheta,
ax*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) + ay*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) + az*cos_correctedPhi*cos_correctedTheta, ay*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta - ax*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - az*sin_correctedPhi*sin_correctedTheta, ay*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) - ax*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta), 0, 1, 0, 0, 0, 0, - ax*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - ay*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - az*cos_correctedPhi*cos_correctedTheta, az*sin_correctedPhi*sin_correctedTheta + ax*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - ay*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta, ax*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - ay*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi),
az*sin_correctedPhi*cos_correctedTheta - ay*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - ax*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi), az*sin_correctedTheta*cos_correctedPhi + ax*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - ay*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta, ay*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - ax*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi), 0, 0, 1, 0, 0, 0, ax*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) + ay*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - az*sin_correctedPhi*cos_correctedTheta, ay*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta - ax*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - az*sin_correctedTheta*cos_correctedPhi, ax*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - ay*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi),
0, hx*sin_correctedTheta*cos_correctedPsi - hz*cos_correctedTheta - hy*sin_correctedPsi*sin_correctedTheta, hy*cos_correctedPsi*cos_correctedTheta + hx*sin_correctedPsi*cos_correctedTheta, 0, 0, 0, 1, 0, 0, 0, hz*cos_correctedTheta - hx*sin_correctedTheta*cos_correctedPsi + hy*sin_correctedPsi*sin_correctedTheta, - hy*cos_correctedPsi*cos_correctedTheta - hx*sin_correctedPsi*cos_correctedTheta,
hx*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) + hy*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) + hz*cos_correctedPhi*cos_correctedTheta, hy*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta - hx*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - hz*sin_correctedPhi*sin_correctedTheta, hy*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) - hx*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta), 0, 0, 0, 0, 1, 0, - hx*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - hy*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - hz*cos_correctedPhi*cos_correctedTheta, hz*sin_correctedPhi*sin_correctedTheta + hx*sin_correctedPhi*cos_correctedPsi*cos_correctedTheta - hy*sin_correctedPhi*sin_correctedPsi*cos_correctedTheta, hx*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hy*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi),
hz*sin_correctedPhi*cos_correctedTheta - hy*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hx*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi), hz*sin_correctedTheta*cos_correctedPhi + hx*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - hy*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta, hy*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi) - hx*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi), 0, 0, 0, 0, 0, 1, hx*(sin_correctedPsi*cos_correctedPhi + sin_correctedPhi*sin_correctedTheta*cos_correctedPsi) + hy*(cos_correctedPhi*cos_correctedPsi - sin_correctedPhi*sin_correctedPsi*sin_correctedTheta) - hz*sin_correctedPhi*cos_correctedTheta, hy*sin_correctedPsi*cos_correctedPhi*cos_correctedTheta - hx*cos_correctedPhi*cos_correctedPsi*cos_correctedTheta - hz*sin_correctedTheta*cos_correctedPhi, hx*(sin_correctedPhi*cos_correctedPsi + sin_correctedPsi*sin_correctedTheta*cos_correctedPhi) - hy*(sin_correctedPhi*sin_correctedPsi - sin_correctedTheta*cos_correctedPhi*cos_correctedPsi),
1, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0,
0, 1, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0,
0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, -1};
arm_mat_init_f32(&H, a, n, H_f32);
/* Matriz Jacobiana transposta para cálculo da confiabilidade do erro . */
float Ht_f32[n*a];
arm_mat_init_f32(&Ht, n, a, Ht_f32);
arm_mat_trans_f32(&H, &Ht);
//Matriz de variâncias
float Racel = buffer_filtro->R_acel;
float Rmag = buffer_filtro->R_mag; //Variância inicial do magnetômetro.
float Rangles = buffer_filtro->R_angles;
float acelModulus = getVectorModulus(medida_accel, 3);
// float magModulus = getVectorModulus(medida_mag, 3);
// float magInitialModulus = getVectorModulus(buffer_filtro->MagInicial, 3);
//Racel += 100*fabsf(1 - acelModulus);
//Rmag += 1*fabs(magInitialModulus - magModulus);
float R_f32[a*a] = {(Racel), 0, 0, 0, 0, 0, 0, 0, 0,
0, (Racel), 0, 0, 0, 0, 0, 0, 0,
0, 0, (Racel), 0, 0, 0, 0, 0, 0,
0, 0, 0, (Rmag), 0, 0, 0, 0, 0,
0, 0, 0, 0, (Rmag), 0, 0, 0, 0,
0, 0, 0, 0, 0, (Rmag), 0, 0, 0,
0, 0, 0, 0, 0, 0, (Rangles), 0, 0,
0, 0, 0, 0, 0, 0, 0, (Rangles), 0,
0, 0, 0, 0, 0, 0, 0, 0, (Rangles)};
arm_mat_init_f32(&R, a, a, R_f32);
//Cálculos do filtro de Kalman
//S = H*P*H' + R
if(arm_mat_mult_f32(&H, &P, &temp_calc_an_0) != ARM_MATH_SUCCESS)
while(1);
if(arm_mat_mult_f32(&temp_calc_an_0, &Ht, &temp_calc_aa_0) != ARM_MATH_SUCCESS)
while(1);
if(arm_mat_add_f32(&temp_calc_aa_0, &R, &S) != ARM_MATH_SUCCESS)
while(1);
//Sinv = inv(S);
//if(arm_mat_inverse_f32(&S, &Sinv) == ARM_MATH_SINGULAR)
// while(1);
arm_mat_inverse_f32(&S, &Sinv);
//Kk = P*Ht*S^(-1)
//P*Ht
if(arm_mat_mult_f32(&P, &Ht, &temp_calc_na_0) != ARM_MATH_SUCCESS)
while(1);
if(arm_mat_mult_f32(&temp_calc_na_0, &Sinv, &K) != ARM_MATH_SUCCESS)
while(1);
//temp_calc_n11 = Kk*y
if(arm_mat_mult_f32(&K, &ykMatrix, &temp_calc_n1_0) != ARM_MATH_SUCCESS)
while(1);
//X = X + temp_calc_n1_1;
if(arm_mat_add_f32(&X, &temp_calc_n1_0, &temp_calc_n1_1) != ARM_MATH_SUCCESS)
while(1);
arm_copy_f32(temp_calc_n1_1_f32, X_f32, n);
//P = (I-K*H)*P
//Matriz identidade para atualização da matriz P à posteriori.
float I_f32[n*n] = { 1,0,0,0,0,0,0,0,0,0,0,0,
0,1,0,0,0,0,0,0,0,0,0,0,
0,0,1,0,0,0,0,0,0,0,0,0,
0,0,0,1,0,0,0,0,0,0,0,0,
0,0,0,0,1,0,0,0,0,0,0,0,
0,0,0,0,0,1,0,0,0,0,0,0,
0,0,0,0,0,0,1,0,0,0,0,0,
0,0,0,0,0,0,0,1,0,0,0,0,
0,0,0,0,0,0,0,0,1,0,0,0,
0,0,0,0,0,0,0,0,0,1,0,0,
0,0,0,0,0,0,0,0,0,0,1,0,
0,0,0,0,0,0,0,0,0,0,0,1};
arm_mat_init_f32(&I, n, n, I_f32);
if(arm_mat_mult_f32(&K, &H, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
while(1);
if(arm_mat_sub_f32(&I, &temp_calc_nn_0, &temp_calc_nn_1) != ARM_MATH_SUCCESS)
while(1);
if(arm_mat_mult_f32(&temp_calc_nn_1, &P, &temp_calc_nn_0) != ARM_MATH_SUCCESS)
while(1);
arm_copy_f32(X_f32, buffer_filtro->ultimo_estado, n);
arm_copy_f32(temp_calc_nn_0_f32, buffer_filtro->P, n*n);
}
void EKF_New(EKF_Filter* ekf)
{
float32_t *H = ekf->H_f32;
float32_t *P = ekf->P_f32;
float32_t *Q = ekf->Q_f32;
float32_t *R = ekf->R_f32;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->P, EKF_STATE_DIM, EKF_STATE_DIM, ekf->P_f32);
arm_mat_zero_f32(&ekf->P);
P[0] = P[8] = P[16] = P[24] = EKF_PQ_INITIAL;
P[32] = P[40] = P[48] = EKF_PW_INITIAL;
//
arm_mat_init_f32(&ekf->Q, EKF_STATE_DIM, EKF_STATE_DIM, ekf->Q_f32);
arm_mat_zero_f32(&ekf->Q);
Q[0] = Q[8] = Q[16] = Q[24] = EKF_QQ_INITIAL;
Q[32] = Q[40] = Q[48] = EKF_QW_INITIAL;
//
arm_mat_init_f32(&ekf->R, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->R_f32);
arm_mat_zero_f32(&ekf->R);
R[0] = R[14] = R[28] = R[42] = EKF_RQ_INITIAL;
R[56] = R[70] = R[84] = EKF_RA_INITIAL;
R[98] = R[112] = R[126] = EKF_RW_INITIAL;
R[140] = R[154] = R[168] = EKF_RM_INITIAL;
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->K, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->K_f32);
arm_mat_init_f32(&ekf->KT, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, ekf->KT_f32);
arm_mat_init_f32(&ekf->S, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->S_f32);
//
arm_mat_init_f32(&ekf->F, EKF_STATE_DIM, EKF_STATE_DIM, ekf->F_f32);
arm_mat_zero_f32(&ekf->F);
arm_mat_identity_f32(&ekf->F, 1.0f);
//
arm_mat_init_f32(&ekf->FT, EKF_STATE_DIM, EKF_STATE_DIM, ekf->FT_f32);
//
arm_mat_init_f32(&ekf->H, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, H);
arm_mat_zero_f32(&ekf->H);
H[0] = H[8] = H[16] = H[24] = 1.0f; //q row 0~3, col 0~3
H[53] = H[61] = H[69] = 1.0f; //w row 7~9, col 4~6
//
arm_mat_init_f32(&ekf->HT, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->HT_f32);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->I, EKF_STATE_DIM, EKF_STATE_DIM, ekf->I_f32);
arm_mat_zero_f32(&ekf->I);
arm_mat_identity_f32(&ekf->I, 1.0f);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->X, EKF_STATE_DIM, 1, ekf->X_f32);
arm_mat_zero_f32(&ekf->X);
//////////////////////////////////////////////////////////////////////////
arm_mat_init_f32(&ekf->Y, EKF_MEASUREMENT_DIM, 1, ekf->Y_f32);
arm_mat_zero_f32(&ekf->Y);
//////////////////////////////////////////////////////////////////////////
//
arm_mat_init_f32(&ekf->tmpP, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpP_f32);
arm_mat_init_f32(&ekf->tmpX, EKF_STATE_DIM, 1, ekf->tmpX_f32);
arm_mat_init_f32(&ekf->tmpYX, EKF_MEASUREMENT_DIM, EKF_STATE_DIM, ekf->tmpYX_f32);
arm_mat_init_f32(&ekf->tmpXY, EKF_STATE_DIM, EKF_MEASUREMENT_DIM, ekf->tmpXY_f32);
arm_mat_init_f32(&ekf->tmpXX, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpXX_f32);
arm_mat_init_f32(&ekf->tmpXXT, EKF_STATE_DIM, EKF_STATE_DIM, ekf->tmpXXT_f32);
arm_mat_init_f32(&ekf->tmpS, EKF_MEASUREMENT_DIM, EKF_MEASUREMENT_DIM, ekf->tmpS_f32);
//////////////////////////////////////////////////////////////////////////
}
void SRCKF_New(SRCKF_Filter* srckf)
{
float32_t kesi;
arm_matrix_instance_f32 KesiPuls, KesiMinu;
float32_t KesiPuls_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM], KesiMinus_f32[SRCKF_STATE_DIM * SRCKF_STATE_DIM];
float32_t *S = srckf->S_f32;
float32_t *SQ = srckf->SQ_f32;
float32_t *SR = srckf->SR_f32;
//////////////////////////////////////////////////////////////////////////
//initialise weight
srckf->W = 1.0f / (float32_t)SRCKF_CP_POINTS;
arm_sqrt_f32(srckf->W, &srckf->SW);
//initialise kesi
//generate the cubature point
kesi = (float32_t)SRCKF_STATE_DIM;
arm_sqrt_f32(kesi, &kesi);
arm_mat_init_f32(&KesiPuls, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiPuls_f32);
arm_mat_init_f32(&KesiMinu, SRCKF_STATE_DIM, SRCKF_STATE_DIM, KesiMinus_f32);
arm_mat_identity_f32(&KesiPuls, kesi);
arm_mat_identity_f32(&KesiMinu, -kesi);
arm_mat_init_f32(&srckf->Kesi, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->Kesi_f32);
arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiPuls, 0, 0);
arm_mat_setsubmatrix_f32(&srckf->Kesi, &KesiMinu, 0, SRCKF_STATE_DIM);
arm_mat_init_f32(&srckf->iKesi, SRCKF_STATE_DIM, 1, srckf->iKesi_f32);
//initialise state covariance
arm_mat_init_f32(&srckf->S, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->S_f32);
arm_mat_zero_f32(&srckf->S);
S[0] = S[8] = S[16] = S[24] = SRCKF_PQ_INITIAL;
S[32] = S[40] = S[48] = SRCKF_PW_INITIAL;
arm_mat_init_f32(&srckf->ST, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->ST_f32);
//initialise measurement covariance
arm_mat_init_f32(&srckf->SY, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SY_f32);
arm_mat_init_f32(&srckf->SYI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYI_f32);
arm_mat_init_f32(&srckf->SYT, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYT_f32);
arm_mat_init_f32(&srckf->SYTI, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SYTI_f32);
arm_mat_init_f32(&srckf->PXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->PXY_f32);
arm_mat_init_f32(&srckf->tmpPXY, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->tmpPXY_f32);
//initialise SQ
arm_mat_init_f32(&srckf->SQ, SRCKF_STATE_DIM, SRCKF_STATE_DIM, srckf->SQ_f32);
arm_mat_zero_f32(&srckf->SQ);
SQ[0] = SQ[8] = SQ[16] = SQ[24] = SRCKF_QQ_INITIAL;
SQ[32] = SQ[40] = SQ[48] = SRCKF_QW_INITIAL;
//initialise SR
arm_mat_init_f32(&srckf->SR, SRCKF_MEASUREMENT_DIM, SRCKF_MEASUREMENT_DIM, srckf->SR_f32);
arm_mat_zero_f32(&srckf->SR);
SR[0] = SR[7] = SR[14] = SRCKF_RA_INITIAL;
SR[21] = SR[28] = SR[35] = SRCKF_RM_INITIAL;
//other stuff
//cubature points
arm_mat_init_f32(&srckf->XCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCP_f32);
//propagated cubature points
arm_mat_init_f32(&srckf->XPCP, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XPCP_f32);
arm_mat_init_f32(&srckf->YPCP, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YPCP_f32);
arm_mat_init_f32(&srckf->XCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->XCPCM_f32);
arm_mat_init_f32(&srckf->tmpXCPCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS, srckf->tmpXCPCM_f32);
arm_mat_init_f32(&srckf->YCPCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS, srckf->YCPCM_f32);
arm_mat_init_f32(&srckf->YCPCMT, SRCKF_CP_POINTS, SRCKF_MEASUREMENT_DIM, srckf->YCPCMT_f32);
arm_mat_init_f32(&srckf->XCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_STATE_DIM, srckf->XCM_f32);
//initialise fill SQ
arm_mat_setsubmatrix_f32(&srckf->XCM, &srckf->SQ, 0, SRCKF_CP_POINTS);
arm_mat_init_f32(&srckf->YCM, SRCKF_MEASUREMENT_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->YCM_f32);
//initialise fill SR
arm_mat_setsubmatrix_f32(&srckf->YCM, &srckf->SR, 0, SRCKF_CP_POINTS);
arm_mat_init_f32(&srckf->XYCM, SRCKF_STATE_DIM, SRCKF_CP_POINTS + SRCKF_MEASUREMENT_DIM, srckf->XYCM_f32);
//Kalman gain
arm_mat_init_f32(&srckf->K, SRCKF_STATE_DIM, SRCKF_MEASUREMENT_DIM, srckf->K_f32);
//////////////////////////////////////////////////////////////////////////
//state vector
arm_mat_init_f32(&srckf->X, SRCKF_STATE_DIM, 1, srckf->X_f32);
arm_mat_zero_f32(&srckf->X);
//measurement vector
arm_mat_init_f32(&srckf->Y, SRCKF_MEASUREMENT_DIM, 1, srckf->Y_f32);
arm_mat_zero_f32(&srckf->Y);
arm_mat_init_f32(&srckf->tmpY, SRCKF_MEASUREMENT_DIM, 1, srckf->tmpY_f32);
arm_mat_zero_f32(&srckf->tmpY);
}
arm_matrix_instance_f32 c_rc_LQR2_errorStateVector_I(pv_type_datapr_attitude attitude,
pv_type_datapr_attitude attitude_reference,
pv_type_datapr_position position,
pv_type_datapr_position position_reference,
pv_type_datapr_servos servo,
pv_type_datapr_servos servo_reference,
float sample_time, bool stop){
arm_matrix_instance_f32 c_rc_LQR2_error_state_vector, c_rc_LQR2_state_vector, c_rc_LQR2_reference_state_vector;
//Integradores
//Vector integration of error(Trapezoidal method)
if (stop){
c_rc_LQR2_deltaxsi[0]=0;
c_rc_LQR2_deltaxsiant[0]=0;
c_rc_LQR2_xsi[0]=0;
c_rc_LQR2_xsiant[0]=0;
c_rc_LQR2_xsi[1]=0;
c_rc_LQR2_xsiant[1]=0;
c_rc_LQR2_deltaxsi[1]=0;
c_rc_LQR2_deltaxsiant[1]=0;
}else{
c_rc_LQR2_deltaxsi[0]=position.z-position_reference.z;
c_rc_LQR2_xsi[0]=c_rc_LQR2_xsiant[0]+(((float32_t)sample_time)*(c_rc_LQR2_deltaxsi[0]+c_rc_LQR2_deltaxsiant[0])*0.5);
c_rc_LQR2_deltaxsi[1]=attitude.yaw-attitude_reference.yaw;
c_rc_LQR2_xsi[1]=c_rc_LQR2_xsiant[1]+(((float32_t)sample_time)*(c_rc_LQR2_deltaxsi[1]+c_rc_LQR2_deltaxsiant[1])*0.5);
}
// anti_windup
c_rc_LQR2_xsi[0]=c_rc_LQR2_saturation(c_rc_LQR2_xsi[0],-0.2,0.2);
c_rc_LQR2_xsi[1]=c_rc_LQR2_saturation(c_rc_LQR2_xsi[1],-0.2,0.2);
//update
c_rc_LQR2_deltaxsiant[0]=c_rc_LQR2_deltaxsi[0]=0;
c_rc_LQR2_deltaxsiant[1]=c_rc_LQR2_deltaxsi[1]=0;
c_rc_LQR2_xsiant[1]=c_rc_LQR2_xsi[0];
c_rc_LQR2_xsiant[1]=c_rc_LQR2_xsi[1];
//State Vector
c_rc_LQR2_state_vector_I_f32[STATE_X]=position.x;
c_rc_LQR2_state_vector_I_f32[STATE_Y]=position.y;
c_rc_LQR2_state_vector_I_f32[STATE_Z]=position.z;
c_rc_LQR2_state_vector_I_f32[STATE_ROLL]=attitude.roll;
c_rc_LQR2_state_vector_I_f32[STATE_PITCH]=attitude.pitch;
c_rc_LQR2_state_vector_I_f32[STATE_YAW]=attitude.yaw;
c_rc_LQR2_state_vector_I_f32[STATE_ALPHA_R]=servo.alphar;
c_rc_LQR2_state_vector_I_f32[STATE_ALPHA_L]=servo.alphal;
c_rc_LQR2_state_vector_I_f32[STATE_DX]=position.dotX;
c_rc_LQR2_state_vector_I_f32[STATE_DY]=position.dotY;
c_rc_LQR2_state_vector_I_f32[STATE_DZ]=position.dotZ;
c_rc_LQR2_state_vector_I_f32[STATE_DROLL]=attitude.dotRoll;
c_rc_LQR2_state_vector_I_f32[STATE_DPITCH]=attitude.dotPitch;
c_rc_LQR2_state_vector_I_f32[STATE_DYAW]=attitude.dotYaw;
c_rc_LQR2_state_vector_I_f32[STATE_DALPHA_R]=servo.dotAlphar;
c_rc_LQR2_state_vector_I_f32[STATE_DALPHA_L]=servo.dotAlphal;
c_rc_LQR2_state_vector_I_f32[STATE_INT_Z]=c_rc_LQR2_xsi[0];
c_rc_LQR2_state_vector_I_f32[STATE_INT_YAW]=c_rc_LQR2_xsi[1];
//Updates the height equilibrium point according to the reference
c_rc_LQR2_state_vector_reference_I_f32[STATE_X]=position_reference.x;
c_rc_LQR2_state_vector_reference_I_f32[STATE_Y]=position_reference.y;
c_rc_LQR2_state_vector_reference_I_f32[STATE_Z]=position_reference.z;
c_rc_LQR2_state_vector_reference_I_f32[STATE_ROLL]=attitude_reference.roll;
c_rc_LQR2_state_vector_reference_I_f32[STATE_PITCH]=attitude_reference.pitch;
c_rc_LQR2_state_vector_reference_I_f32[STATE_YAW]=attitude_reference.yaw;
c_rc_LQR2_state_vector_reference_I_f32[STATE_ALPHA_R]=servo_reference.alphar;
c_rc_LQR2_state_vector_reference_I_f32[STATE_ALPHA_L]=servo_reference.alphal;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DX]=position_reference.dotX;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DY]=position_reference.dotY;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DZ]=position_reference.dotZ;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DROLL]=attitude_reference.dotRoll;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DPITCH]=attitude_reference.dotPitch;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DYAW]=attitude_reference.dotYaw;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DALPHA_R]=servo_reference.dotAlphar;
c_rc_LQR2_state_vector_reference_I_f32[STATE_DALPHA_L]=servo_reference.dotAlphal;
c_rc_LQR2_state_vector_reference_I_f32[STATE_INT_Z]=0;
c_rc_LQR2_state_vector_reference_I_f32[STATE_INT_YAW]=0;
//Initializes the matrices
arm_mat_init_f32(&c_rc_LQR2_state_vector, 18, 1, (float32_t *)c_rc_LQR2_state_vector_I_f32);
arm_mat_init_f32(&c_rc_LQR2_reference_state_vector, 18, 1, (float32_t *)c_rc_LQR2_state_vector_reference_I_f32);
arm_mat_init_f32(&c_rc_LQR2_error_state_vector, 18, 1, (float32_t *)c_rc_LQR2_error_state_vector_I_f32);
//e(t)=x(k)- xr(k)
arm_mat_sub_f32(&c_rc_LQR2_state_vector, &c_rc_LQR2_reference_state_vector, &c_rc_LQR2_error_state_vector);
return c_rc_LQR2_error_state_vector;
}
void EKF_Attitude::innovate_priori(Quaternion& quaternion,
float Gx,
float Gy,
float Gz){
float quaternion_array[4] = {quaternion.w, quaternion.x, quaternion.y, quaternion.z};
arm_matrix_instance_f32 q;
arm_mat_init_f32(&q, 4, 1, quaternion_array);
static uint32_t timestamp;
static float dt;
dt = Time.get_time_since_sec(timestamp);
//---------------------- PREDICTION -----------------------------
//1. Obtain the angular velocities:
float wx = (Gx*2*PI)/360.0;
float wy = (Gy*2*PI)/360.0;
float wz = (Gz*2*PI)/360.0;
//2. Calculate the discrete time state transition matrix:
float32_t omega_f32[16] = {
0, -wx, -wy, -wz,
wx, 0, wz, -wy,
wy, -wz, 0, wx,
wz, wy, -wx, 0};
arm_matrix_instance_f32 omega; /* Matrix Omega Instance */
arm_mat_init_f32(&omega, 4, 4, omega_f32);
//Ak = I + 0.5*omega*T;
arm_mat_scale_f32(&omega, 0.5*dt, &temp4x4);
arm_mat_add_f32(&I, &temp4x4, &Ak);
//3. Calculation of the a priori system state:
// q = Ak*q
arm_mat_mult_f32(&Ak, &q, &temp4x1);
//memcpy((void*)quaternion, (void*)temp4x1_f32, 4);
/*quaternion[0] = temp4x1_f32[0];
quaternion[1] = temp4x1_f32[1];
quaternion[2] = temp4x1_f32[2];
quaternion[3] = temp4x1_f32[3];*/
quaternion.w = temp4x1_f32[0];
quaternion.x = temp4x1_f32[1];
quaternion.y = temp4x1_f32[2];
quaternion.z = temp4x1_f32[3];
//4. Calculation of the a proiri noise covariance matrix:
// Pk = Ak*Pk*Ak.' + Qk;
arm_mat_trans_f32(&Ak, &Ak_trans);
//temp = Ak*Pk
arm_mat_mult_f32(&Ak, &Pk, &temp4x4);
//Pk = temp*Ak.'
arm_mat_mult_f32(&temp4x4, &Ak_trans, &Pk);
//temp = Pk + Qk:
arm_mat_add_f32(&Pk, &Qk, &temp4x4);
//Pk = temp;
copy_matrix(&Pk, &temp4x4);
timestamp = Time.get_timestamp();
}
EKF_Attitude::EKF_Attitude(){
//Parameters
arm_mat_init_f32(&Pk, 4, 4, Pk_f32);
arm_mat_init_f32(&R, 3, 3, R_f32);
arm_mat_init_f32(&Qk, 4, 4, Qk_f32);
arm_mat_init_f32(&I, 4, 4, I_f32);
arm_mat_init_f32(&x, 2, 1, x_f32);
//Helpers:
arm_mat_init_f32(&Ak, 4, 4, Ak_f32);
arm_mat_init_f32(&Ak_trans, 4, 4, Ak_trans_f32);
arm_mat_init_f32(&S, 3, 3, S_f32);
arm_mat_init_f32(&K_k1, 4, 3, K_k1_f32);
arm_mat_init_f32(&K_k2, 4, 3, K_k2_f32);
arm_mat_init_f32(&h1_q, 3, 1, h1_q_f32);
arm_mat_init_f32(&q_corr, 4, 1, q_corr_f32);
arm_mat_init_f32(&q_corr_2, 4, 1, q_corr_2_f32);
//Temps
arm_mat_init_f32(&temp4x4, 4, 4, temp4x4_f32);
arm_mat_init_f32(&temp4x4_2, 4, 4, temp4x4_2_f32);
arm_mat_init_f32(&temp3x4, 3, 4, temp3x4_f32);
arm_mat_init_f32(&temp4x3, 4, 3, temp4x3_f32);
arm_mat_init_f32(&temp3x3, 3, 3, temp3x3_f32);
arm_mat_init_f32(&temp3x1, 3, 1, temp3x1_f32);
arm_mat_init_f32(&temp4x1, 4, 1, temp4x1_f32);
arm_mat_init_f32(&null4x4, 4, 4, null4x4_f32);
//arm_mat_scale_f32(&R, 0.0001, &temp3x3);
//copy_matrix(&R, &temp3x3);
arm_mat_scale_f32(&Qk, 0.0001, &temp4x4);
copy_matrix(&Qk, &temp4x4);
}
void EKF_Attitude::innovate_stage2(Quaternion& quaternion,
float Mx,
float My,
float Mz){
float quaternion_array[4] = {quaternion.w, quaternion.x, quaternion.y, quaternion.z};
arm_matrix_instance_f32 q;
arm_mat_init_f32(&q, 4, 1, quaternion_array);
//---------------------- MEASUREMENT UPDATE -----------------------------
//1: Calculation of the Jacobian matrix:
float32_t H_k2_f32[16] = {
2*quaternion.z, 2*quaternion.y, 2*quaternion.x, 2*quaternion.w,
2*quaternion.w, -2*quaternion.x, -2*quaternion.y, -2*quaternion.z,
-2*quaternion.x, -2*quaternion.w, 2*quaternion.z, 2*quaternion.y
};
arm_matrix_instance_f32 H_k2; /* Matrix Omega Instance */
arm_mat_init_f32(&H_k2, 3, 4, H_k2_f32);
//2: Calculate the Kalman Gain:
/* S = inv(H_k2 * Pk * H_k2.' + V_k2 * R_k2 * V_k2.');
K_k1 = Pk * H_k2.' * S;
*/
//temp3x3 = H_k2 * Pk * H_k2.'
arm_mat_trans_f32(&H_k2, &temp4x3);
arm_mat_mult_f32(&H_k2, &Pk, &temp3x4);
arm_mat_mult_f32(&temp3x4, &temp4x3, &temp3x3);
//temp3x3 = inv(temp3x3 + R) ( = S!!)
arm_mat_add_f32(&temp3x3, &R, &S);
arm_mat_inverse_f32(&S, &temp3x3);
//K_k2 = Pk * H_k2.' (temp4x3 = H_k2.')
arm_mat_mult_f32(&Pk, &temp4x3, &K_k2);
arm_mat_mult_f32(&K_k2, &temp3x3, &temp4x3);
copy_matrix(&K_k2, &temp4x3);
//3: Reading of the magnetometer data:
float32_t z_k2_f32[3] = {
Mx,
My,
Mz,
};
arm_matrix_instance_f32 z_k2; /* Matrix Omega Instance */
arm_mat_init_f32(&z_k2, 3, 1, z_k2_f32);
//4: Calculation of h2(q):
float32_t h2_q_f32[3] = {
2*quaternion.x*quaternion.y + 2*quaternion.w*quaternion.z,
quaternion.w*quaternion.w - quaternion.x*quaternion.x- quaternion.y*quaternion.y - quaternion.z*quaternion.z,
2*quaternion.y*quaternion.z - 2*quaternion.w*quaternion.x
};
arm_matrix_instance_f32 h2_q; /* Matrix Omega Instance */
arm_mat_init_f32(&h2_q, 3, 1, h2_q_f32);
//5: Calculation of the correction factor
//q_corr_2 = K_k2*(z_k2 - h2_q);
arm_mat_sub_f32(&z_k2, &h2_q, &temp3x1);
arm_mat_mult_f32(&K_k2, &temp3x1, &q_corr_2);
q_corr_2.pData[1] = 0;
q_corr_2.pData[2] = 0;
//6: Calculation of the a posteriori state estimation
//q = q + q_2;
arm_mat_add_f32(&q, &q_corr_2, &temp4x1);
copy_matrix(&q, &temp4x1);
//7: Calculation of a aposteriori error covariance matrix
//Pk = (I - K_k2 * H_k2)*Pk;
arm_mat_mult_f32(&K_k2, &H_k2, &temp4x4);
arm_mat_sub_f32(&I, &temp4x4, &temp4x4_2);
arm_mat_mult_f32(&temp4x4_2, &Pk, &temp4x4);
copy_matrix(&Pk, &temp4x4);
quaternion.w = quaternion_array[0];
quaternion.x = quaternion_array[1];
quaternion.y = quaternion_array[2];
quaternion.z = quaternion_array[3];
}
void EKF_Attitude::innovate_stage1(Quaternion& quaternion,
float Ax,
float Ay,
float Az){
float quaternion_array[4] = {quaternion.w, quaternion.x, quaternion.y, quaternion.z};
arm_matrix_instance_f32 q;
arm_mat_init_f32(&q, 4, 1, quaternion_array);
//---------------------- MEASUREMENT UPDATE -----------------------------
//1: Calculation of the Jacobian matrix:
float32_t H_k1_f32[16] = {
-2*quaternion.y, 2*quaternion.z, -2*quaternion.w, 2*quaternion.x,
2*quaternion.x, 2*quaternion.w, 2*quaternion.z, 2*quaternion.y,
2*quaternion.w, -2*quaternion.x, -2*quaternion.y, 2*quaternion.z
};
arm_matrix_instance_f32 H_k1; /* Matrix Omega Instance */
arm_mat_init_f32(&H_k1, 3, 4, H_k1_f32);
//2: Calculate the Kalman Gain:
/* S = inv(H_k1 * Pk * H_k1.' + V_k1*R_k1*V_k1.');
K_k1 = Pk * H_k1.' * S;
*/
//temp3x3 = H_k1 * Pk * H_k1.'
arm_mat_trans_f32(&H_k1, &temp4x3);
arm_mat_mult_f32(&H_k1, &Pk, &temp3x4);
arm_mat_mult_f32(&temp3x4, &temp4x3, &temp3x3);
//temp3x3 = inv(temp3x3 + R) ( = S!!)
arm_mat_add_f32(&temp3x3, &R, &S);
arm_mat_inverse_f32(&S, &temp3x3);
//K_k1 = Pk * H_k1.' (temp4x3 = H_k1.')
arm_mat_mult_f32(&Pk, &temp4x3, &K_k1);
arm_mat_mult_f32(&K_k1, &temp3x3, &temp4x3);
copy_matrix(&K_k1, &temp4x3);
//3: Reading of the accelerometer data:
float32_t z_f32[3] = {
Ax,
Ay,
Az,
};
arm_matrix_instance_f32 z; /* Matrix Omega Instance */
arm_mat_init_f32(&z, 3, 1, z_f32);
//4: Calculation of h1(q):
float32_t h1_q_f32[3] = {
2*quaternion.x*quaternion.z-2*quaternion.w*quaternion.y,
2*quaternion.w*quaternion.x+2*quaternion.y*quaternion.z,
quaternion.w*quaternion.w - quaternion.x*quaternion.x- quaternion.y*quaternion.y + quaternion.z*quaternion.z
};
arm_matrix_instance_f32 h1_q; /* Matrix Omega Instance */
arm_mat_init_f32(&h1_q, 3, 1, h1_q_f32);
//5: Calculation of the correction factor
//q_corr = K_k1*(z - h1_q);
arm_mat_sub_f32(&z, &h1_q, &temp3x1);
arm_mat_mult_f32(&K_k1, &temp3x1, &q_corr);
q_corr.pData[3] = 0;
//6: Calculation of the a posteriori state estimation
//q = q + q_1;
arm_mat_add_f32(&q, &q_corr, &temp4x1);
copy_matrix(&q, &temp4x1);
//7: Calculation of a aposteriori error covariance matrix
//Pk = (I - K_k1 * H_k1)*Pk;
arm_mat_mult_f32(&K_k1, &H_k1, &temp4x4);
arm_mat_sub_f32(&I, &temp4x4, &temp4x4_2);
arm_mat_mult_f32(&temp4x4_2, &Pk, &temp4x4);
copy_matrix(&Pk, &temp4x4);
quaternion.w = quaternion_array[0];
quaternion.x = quaternion_array[1];
quaternion.y = quaternion_array[2];
quaternion.z = quaternion_array[3];
}
