#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>

#define SIGMA  0.01f
#define SIGMA_V  0.005f

void invert3x3(float * src, float * dst);

void multiply3x3(float *a, float *b, float *dst);

void multiply3x1(float *a, float *b, float *dst);

void init_attitude_matrix(float *C, float *acc, float g);

void reset_yaw_C(float *C);

void State_covariance_matrix_update(float *P, float *acc_n, float dt);

void Kalfman_gain(float *P, float *Temporary_array, float *Temporary_array1, float *K);

void Kalfman_gain_angle(float *P, float *Temporary_array, float *Temporary_array1, float *K);

void multiply9x3(float *K, float *vel_n, float* delta_x);

void State_covariance_matrix_corr(float *P, float *P_tmp, float *K);

void State_covariance_matrix_corr_angle(float *P, float *P_tmp, float *K);

void Att_matrix_corr(float *C, float *C_prev, float *Temporary_array, float *Temporary_array1, float *delta_x);

void pos_n_corr(float *pos_n, float *delta_x);

void vel_n_corr(float *vel_n, float *delta_x);

void State_covariance_matrix_orthogonalization(float *P);