public static MatrixFixed dR_by_dqz(Quaternion q)
{
float q0 = q.R();
float qx = q.X();
float qy = q.Y();
float qz = q.Z();
float[] a = new float[9];
a[0] = -2*qz;
a[1] = -2*q0;
a[2] = 2*qx;
a[3] = 2*q0;
a[4] = -2*qz;
a[5] = 2*qy;
a[6] = 2*qx;
a[7] = 2*qy;
a[8] = 2*qz;
MatrixFixed M = new MatrixFixed(3,3);
return M;
}
public static MatrixFixed dq3_by_dq2(Quaternion q1)
{
float[] a = new float[16];
a[0] = q1.R();
a[1] = -q1.X();
a[2] = -q1.Y();
a[3] = -q1.Z();
a[4] = q1.X();
a[5] = q1.R();
a[6] = q1.Z();
a[7] = -q1.Y();
a[8] = q1.Y();
a[9] = -q1.Z();
a[10] = q1.R();
a[11] = q1.X();
a[12] = q1.Z();
a[13] = q1.Y();
a[14] = -q1.X();
a[15] = q1.R();
MatrixFixed M = new MatrixFixed(a);
return M;
}