jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Import Freescale's Kalman filter

Browse source
This commit is contained in:
PaulStoffregen 2016-03-12 09:46:09 -08:00
commit dd59a04598
5 changed files with 1531 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

9
visualize.c
View file

@ -61,11 +61,10 @@ static void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t
static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
{
float qx = quat->x;
float qy = quat->y;
float qz = quat->z;
float qw = quat->w;
float qw = quat->q0;
float qx = quat->q1;
float qy = quat->q2;
float qz = quat->q3;
rmatrix[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
rmatrix[1] = 2.0f * qx * qy - 2.0f * qz * qw;
rmatrix[2] = 2.0f * qx * qz + 2.0f * qy * qw;