Quaternion
Quaternion can be used to describe rigid body rotation or attitude transformation, and its expression is concise compared with direction cosine matrix. A quaternion is a column vector containing four elements. The first element of the column vector represents the angle that the rigid body has rotated around the rotation axis, and the remaining 3 elements represent the direction of the rotation axis.
Attitude Update Algorithm
The recursive calculation of the attitude of this program is realized by quaternion, and the change of attitude is represented by the equivalent rotation vector.
When updating the attitude, the attitude quaternion at k-1 time and the two gyroscope angles at k-1 to k time are used. Incremental sampling, the calculation method is as follows:
The normalization of the quaternion in the program qmulrv()is carried out in the function, and the function of this function is: update the attitude quaternion by using the rotation vector.
In the program, the twin-sample coning error compensation coningcom()is carried out in the function. The function of this function is: to perform coning error compensation and output the compensated rotation vector.
