/* Generated code, see convert_model.py */


// sin(i)*(2*x*z-2*w*y)+cos(i)*(2*w*z+2*x*y)
#define KALMAN_Y_MAG_X (fix16_mul(sin_i, (fix16_mul(2 * x, z) - fix16_mul(2 * w, y))) + fix16_mul(cos_i, (fix16_mul(2 * w, z) + fix16_mul(2 * x, y))))

// cos(i)*(-2*z^2-2*x^2+1)+sin(i)*(2*y*z+2*w*x)
#define KALMAN_Y_MAG_Y (fix16_mul(cos_i, (-2 * (fix16_mul(z, z)) - 2 * (fix16_mul(x, x)) + fix16_from_int(1))) + fix16_mul(sin_i, (fix16_mul(2 * y, z) + fix16_mul(2 * w, x))))

// cos(i)*(2*y*z-2*w*x)+sin(i)*(-2*y^2-2*x^2+1)
#define KALMAN_Y_MAG_Z (fix16_mul(cos_i, (fix16_mul(2 * y, z) - fix16_mul(2 * w, x))) + fix16_mul(sin_i, (-2 * (fix16_mul(y, y)) - 2 * (fix16_mul(x, x)) + fix16_from_int(1))))

// 2*x*z-2*w*y
#define KALMAN_Y_ACC_X (fix16_mul(2 * x, z) - fix16_mul(2 * w, y))

// 2*y*z+2*w*x
#define KALMAN_Y_ACC_Y (fix16_mul(2 * y, z) + fix16_mul(2 * w, x))

// -2*y^2-2*x^2+1
#define KALMAN_Y_ACC_Z (-2 * (fix16_mul(y, y)) - 2 * (fix16_mul(x, x)) + fix16_from_int(1))

// 2*cos(i)*z-2*sin(i)*y
#define KALMAN_H_MAG_X_0 (fix16_mul(2 * cos_i, z) - fix16_mul(2 * sin_i, y))

// 2*sin(i)*z+2*cos(i)*y
#define KALMAN_H_MAG_X_1 (fix16_mul(2 * sin_i, z) + fix16_mul(2 * cos_i, y))

// 2*cos(i)*x-2*sin(i)*w
#define KALMAN_H_MAG_X_2 (fix16_mul(2 * cos_i, x) - fix16_mul(2 * sin_i, w))

// 2*sin(i)*x+2*cos(i)*w
#define KALMAN_H_MAG_X_3 (fix16_mul(2 * sin_i, x) + fix16_mul(2 * cos_i, w))

// cos(i)*(2*x*z-2*w*y)-sin(i)*(2*w*z+2*x*y)
#define KALMAN_H_MAG_X_4 (fix16_mul(cos_i, (fix16_mul(2 * x, z) - fix16_mul(2 * w, y))) - fix16_mul(sin_i, (fix16_mul(2 * w, z) + fix16_mul(2 * x, y))))

// 2*sin(i)*x
#define KALMAN_H_MAG_Y_0 (fix16_mul(2 * sin_i, x))

// 2*sin(i)*w-4*cos(i)*x
#define KALMAN_H_MAG_Y_1 (fix16_mul(2 * sin_i, w) - fix16_mul(4 * cos_i, x))

// 2*sin(i)*z
#define KALMAN_H_MAG_Y_2 (fix16_mul(2 * sin_i, z))

// 2*sin(i)*y-4*cos(i)*z
#define KALMAN_H_MAG_Y_3 (fix16_mul(2 * sin_i, y) - fix16_mul(4 * cos_i, z))

// cos(i)*(2*y*z+2*w*x)-sin(i)*(-2*z^2-2*x^2+1)
#define KALMAN_H_MAG_Y_4 (fix16_mul(cos_i, (fix16_mul(2 * y, z) + fix16_mul(2 * w, x))) - fix16_mul(sin_i, (-2 * (fix16_mul(z, z)) - 2 * (fix16_mul(x, x)) + fix16_from_int(1))))

// -2*cos(i)*x
#define KALMAN_H_MAG_Z_0 (-fix16_mul(2 * cos_i, x))

// -4*sin(i)*x-2*cos(i)*w
#define KALMAN_H_MAG_Z_1 (-fix16_mul(4 * sin_i, x) - fix16_mul(2 * cos_i, w))

// 2*cos(i)*z-4*sin(i)*y
#define KALMAN_H_MAG_Z_2 (fix16_mul(2 * cos_i, z) - fix16_mul(4 * sin_i, y))

// 2*cos(i)*y
#define KALMAN_H_MAG_Z_3 (fix16_mul(2 * cos_i, y))

// cos(i)*(-2*y^2-2*x^2+1)-sin(i)*(2*y*z-2*w*x)
#define KALMAN_H_MAG_Z_4 (fix16_mul(cos_i, (-2 * (fix16_mul(y, y)) - 2 * (fix16_mul(x, x)) + fix16_from_int(1))) - fix16_mul(sin_i, (fix16_mul(2 * y, z) - fix16_mul(2 * w, x))))

// -2*y
#define KALMAN_H_ACC_X_0 (-2 * y)

// 2*z
#define KALMAN_H_ACC_X_1 (2 * z)

// -2*w
#define KALMAN_H_ACC_X_2 (-2 * w)

// 2*x
#define KALMAN_H_ACC_X_3 (2 * x)

// 0
#define KALMAN_H_ACC_X_4 (fix16_from_int(0))

// 2*x
#define KALMAN_H_ACC_Y_0 (2 * x)

// 2*w
#define KALMAN_H_ACC_Y_1 (2 * w)

// 2*z
#define KALMAN_H_ACC_Y_2 (2 * z)

// 2*y
#define KALMAN_H_ACC_Y_3 (2 * y)

// 0
#define KALMAN_H_ACC_Y_4 (fix16_from_int(0))

// 0
#define KALMAN_H_ACC_Z_0 (fix16_from_int(0))

// -4*x
#define KALMAN_H_ACC_Z_1 (-4 * x)

// -4*y
#define KALMAN_H_ACC_Z_2 (-4 * y)

// 0
#define KALMAN_H_ACC_Z_3 (fix16_from_int(0))

// 0
#define KALMAN_H_ACC_Z_4 (fix16_from_int(0))