JP5405417B2 - Attitude angle stabilization device and method - Google Patents

Description

  The present invention relates to a posture angle stabilization apparatus and method.

  Conventionally, Kalman filters have been widely used to estimate errors in GPS combined navigation systems that calculate the position, velocity, and direction of a moving object by GPS (Global Positioning System) / IMU (Inertial Measurement Unit) combined navigation. ing. In the Kalman filter, the position, speed, direction, and the like of the moving object are estimated by the model, but the model is greatly different between the moving object and the stationary object. In particular, when the moving body is stationary, the estimation accuracy of the Kalman filter is deteriorated due to the observability problem of the observation amount. When the moving body continues to be stationary (for example, the moving body stops at a red signal), there is a problem that the position, speed, direction, and the like of the moving body gradually drift. Also, if the model is switched between moving and stationary, the attitude angle and speed responsiveness deteriorates when switching from a stationary state to a dynamic state, resulting in discontinuous output.

  With respect to such a problem, a technology is disclosed in which the GPS combined navigation apparatus eliminates changes in the estimated position, estimated speed, and estimated direction when the moving object is stationary, and at the same time eliminates the deterioration in responsiveness when moving from stationary. Patent Document 1 is known. The GPS combined navigation apparatus disclosed in Patent Document 1 includes a stationary determination unit that determines whether a moving body is stationary. When the stationary determination unit determines that the moving body is stationary, the observation model used for the observation update of the Kalman filter is changed. At the same time, the change in the error covariance matrix due to the update is corrected.

JP 2008-232869 A

  Such a technology has been proposed, but in a high-accuracy speed measurement device using GPS / IMU combined navigation, the change in position, speed, and direction when the moving body is stationary, and when switching from stationary to moving Even so, there is a need for a device that can perform more continuous posture angle and velocity response output than the prior art.

  The present invention uses a GPS receiver and is incorporated into a high-accuracy speed measurement device using a Kalman filter to eliminate changes in the position, speed, and direction of a moving object when it is stationary, and to switch from a stationary state to a dynamic state. It is an object of the present invention to provide a posture angle stabilization device and method that can perform continuous posture angle and velocity response output even at times.

  The present invention provides the following solutions.

  (1) The speed of the moving body is estimated by the Kalman filter from the measured speed measured using the GPS receiver and the IMU, and the speed of the moving body is calculated by the autonomous navigation algorithm by combining the estimated speed and the measured speed. A posture angle stabilization device included in the device, the posture angle change amount calculating means for calculating a change amount of the posture angle from the angular velocity of the moving body measured using the IMU and the feedback correction value; An integration processing unit that integrates the amount of change in posture angle calculated by the posture angle change amount calculation unit, a posture angle integrated by the integration processing unit, and an estimated value of the posture angle estimated by the Kalman filter. The Kalman filter attitude correction processing means for generating and outputting an attitude matrix and the velocity measured by the GPS receiver are less than a certain threshold value. In addition, the speed turning determination unit that determines whether or not the angular velocity measured using the IMU is less than a certain threshold, and the speed or angular velocity is equal to or greater than a certain threshold by the speed turning determination unit. When it is determined, the posture matrix generated by the Kalman filter posture correction processing means is latched, and when it is determined that the velocity and the angular velocity are less than a certain threshold, the generated posture matrix is latched. Posture angle latching means for holding a latch of the posture matrix before being determined to be less than the threshold value, transposition processing means for calculating a transposition matrix of the posture matrix latched by the posture angle latching means, Multiplying the posture matrix transposed by the transposition processing means and the posture matrix generated by the Kalman filter posture correction processing means, To correct the posture angle based on the posture angle error calculation processing means for calculating the difference, the posture angle error calculated by the posture angle error calculation processing means, and the posture matrix generated by the Kalman filter posture correction processing means. And a correction control unit that calculates the correction control value and uses the correction value to feed back to the posture angle change amount calculation unit.

  According to the configuration of (1), the posture angle stabilization device according to the present invention calculates a change amount of the posture angle from the angular velocity of the moving body measured using the IMU and the feedback correction value, and calculates Then, a posture matrix is generated from the integrated posture angle and the estimated posture angle estimated by the Kalman filter, and output. Then, the attitude angle stabilization device determines whether the velocity measured by the GPS receiver is less than a certain threshold value, and whether the angular velocity measured using the IMU is less than a certain threshold value, When it is determined that the velocity or angular velocity is greater than or equal to a certain threshold, the generated posture matrix is latched. When it is determined that the velocity and angular velocity are less than the certain threshold, the generated posture matrix is not latched. Holds the latch of the posture matrix before determining that it is less than the threshold, calculates the transposed matrix of the latched posture matrix, performs the multiplication process of the calculated transpose matrix and the generated posture matrix, and the posture angle An error is calculated, a correction control value for correcting the posture angle is calculated based on the calculated posture angle error and the generated posture matrix, and is set as a correction value to be fed back.

  That is, the posture angle stabilization device according to the present invention determines whether the moving body is moving and stationary, and holds the posture matrix before moving from moving to stationary when the moving body is stationary, and uses the held posture matrix. Then, the posture angle is corrected and controlled, and when the movement is started, the posture matrix is generated using the held posture matrix. Therefore, the attitude angle stabilization device according to the present invention is incorporated in a high-accuracy speed measurement device using a Kalman filter using a GPS receiver, and eliminates changes in the position, velocity, and direction of a moving object when stationary. In addition, even when switching from the stationary state to the dynamic state, it is possible to perform continuous posture angle and speed response output.

  (2) The speed of the moving object is estimated by the Kalman filter from the measured speed measured using the GPS receiver and the IMU, and the speed of the moving object is calculated by the autonomous navigation algorithm by combining the estimated speed and the measured speed. A posture angle change which is a method executed by a posture angle stabilization device included in the device and calculates a change amount of the posture angle from the angular velocity of the moving body measured using the IMU and the feedback correction value. An amount calculation step, an integration processing step for integrating the amount of change in posture angle calculated by the posture angle change amount calculation step, a posture angle integrated by the integration processing step, and a posture angle estimated by the Kalman filter. A Kalman filter attitude correction processing step that generates and outputs an attitude matrix from the estimated value, and the GPS receiver A speed turning determination step for determining whether the measured speed is less than a certain threshold value and an angular velocity measured using the IMU is less than a certain threshold value; and the speed turning determination step, When it is determined that the velocity or angular velocity is equal to or greater than a certain threshold value, the posture matrix generated by the Kalman filter posture correction processing means is latched, and when it is determined that the velocity and angular velocity are less than the certain threshold value A posture angle latch step for holding a latch of a posture matrix before being determined to be less than the threshold without latching the generated posture matrix, and a posture matrix latched by the posture angle latch step. A transposition processing step for calculating a transposition matrix, a transposition matrix obtained by the transposition processing step, and the Kalman filter attitude correction processing A posture angle error calculation processing step for performing a multiplication process with the posture matrix generated in the step and calculating a posture angle error, a posture angle error calculated by the posture angle error calculation processing step, and the Kalman filter posture correction processing step. And a correction control step of calculating a correction control value for correcting the posture angle based on the posture matrix generated by the step and using the correction value as a feedback value to the posture angle change amount calculating step.

  Therefore, as in (1), the method according to the present invention is executed by a posture angle stabilization device incorporated in a high-accuracy speed measurement device using a Kalman filter using a GPS receiver, and the moving object is stationary. It is possible to eliminate changes in position, velocity, and direction, and to perform continuous posture angle and velocity response output even when switching from a stationary state to a dynamic state.

  According to the present invention, in a device that calculates the speed of a moving body using a Kalman filter and an autonomous navigation algorithm from the speed measured using a GPS receiver and an IMU, the posture angle of the moving body that is calculated in a stationary state can be constantly stabilized. it can. Furthermore, the present invention can perform continuous attitude angle and speed response output even when switching from a stationary state to a dynamic state, and as a result, measurement accuracy during low speed and low turn It is possible to improve.

It is a block diagram which shows the structure of the attitude angle stabilization apparatus which is one Embodiment of this invention. It is a figure which shows an example of the hardware constitutions of the attitude | position stabilization apparatus which concerns on one Embodiment of this invention. It is a flowchart which shows the processing content of the attitude | position stabilization apparatus which concerns on one Embodiment of this invention. It is a figure which shows the change of the output of a posture angle in the posture angle stabilization apparatus which concerns on one Embodiment of this invention.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

FIG. 1 is a block diagram showing a configuration of an attitude angle stabilization device 100 according to an embodiment of the present invention. The attitude angle stabilization device 100 includes an attitude angle change amount calculation unit 101, an integration processing unit 102, a Kalman filter attitude correction processing unit 103, a speed turning determination unit 104, an attitude angle latch unit 105, and a transposition processing unit 106. A posture angle error calculation processing unit 107 and a correction control unit 108. The attitude angle stabilization device 100 is incorporated in a strap-down navigator that executes an autonomous navigation algorithm of a high-accuracy speed measurement device (not shown). The high-accuracy velocity measuring device estimates the velocity of the moving body by a Kalman filter from the measured velocity measured using the GPS receiver and the IMU, and fuses the estimated velocity with the measured velocity. In addition, the angular velocity (ω) obtained by the three-axis gyro sensor, the posture angle correction value (δθ) from the Kalman filter, and the integrated velocity (V) of the GPS / IMU are inputted, and the posture angle stabilizing device 100 uses the direction cosine matrix. The expressed attitude matrix (R _new ) is acquired, and the speed of the moving object is calculated by the autonomous navigation algorithm.

  In the high-accuracy velocity measuring device, the three-axis gyro sensor measures the three-dimensional angular velocity of the moving body. The Kalman filter estimates and calculates a correction value of the posture angle from the posture angle.

  The posture angle change amount calculation unit 101 calculates the change amount of the posture angle from the angular velocity of the moving object measured using the IMU and the corrected correction value. That is, the posture angle change amount calculation unit 101 calculates the posture angle change amount by calculating Formula 1.

The integration processing unit 102 integrates the change amount of the posture angle calculated by the posture angle change amount calculation unit 101. The integration processing unit 102 performs integration processing of Formula 2, but performs linear approximation as in Formula 3, for example, and performs discrete integration processing. Here, R _{n + 1} is a posture angle after one step calculation, and R _n is a posture angle one step before.

The Kalman filter posture correction processing unit 103 generates and outputs a posture matrix from the posture angle integrated by the integration processing unit 102 and the estimated value of the posture angle estimated by the Kalman filter. The Kalman filter posture correction processing unit 103 corrects the posture angle based on the position and speed of the GPS according to Equation 4. Here, R _new is a posture matrix after the Kalman filter posture correction processing, I is a 3 × 3 unit matrix, δθ is a posture angle error estimated by the Kalman filter, and × is a cross product.

  The speed turning determination unit 104 determines whether the speed measured by the GPS receiver is less than a certain threshold value and the angular velocity measured using the IMU is less than a certain threshold value. That is, the speed turning determination unit 104 determines that the vehicle is stationary when the speed measured by the GPS receiver is less than a certain threshold value and the angular velocity measured using the IMU is less than the certain threshold value. To do. Similarly, the speed turning determination unit 104 determines whether the speed measured by the GPS receiver is greater than or equal to a certain threshold value or the angular velocity measured using the IMU is greater than or equal to a certain threshold value. Judged as a state.

  For example, when the average value V of the integrated speed by GPS / IMU is less than xkm / h and the average values of the angular velocity outputs ω of the three-axis gyro are all less than ydeg / s, the speed turning determination unit 104 is in a stationary state. And the stationary state flag is output. When the average value V of the integrated speed by the GPS / IMU is xkm / h or more, or the average value of the angular velocity output ω of the three-axis gyro is all ydeg / s or more, the speed turning determination unit 104 Determine and output a dynamic state flag.

The posture angle latch unit 105 latches the posture matrix R _new generated by the Kalman filter posture correction processing unit 103 when the speed / turning determination unit 104 determines that the speed or angular velocity is equal to or greater than a certain threshold value. When it is determined that the velocity and the angular velocity are less than a certain threshold, the posture matrix before being determined to be less than the threshold without latching the posture matrix _Rnew generated by the Kalman filter posture correction processing unit 103 Hold the latch. That is, the attitude angle latch unit 105 determines whether or not to latch the attitude matrix R _new according to the output result of the speed turning determination unit 104, and latches the attitude matrix. For example, when the dynamic state flag is output by the speed turning determination unit 104, the posture angle latch unit 105 latches the posture matrix R _new every time it is generated. When the stationary state flag is output by the speed turning determination unit 104, the posture angle latch unit 105 does not latch the generated posture matrix _Rnew , and the dynamic state flag before the stationary state flag is output. Holds the latched posture matrix.

The transposition processing unit 106 calculates a transpose matrix R _new ^T of the posture matrix latched by the posture angle latch unit 105.

The posture angle error calculation processing unit 107 performs a multiplication process of the transposed matrix R _new ^T calculated by the transposition processing unit 106 and the posture matrix R _new generated by the Kalman filter posture correction processing unit 103 to calculate a posture angle error. To do. Here, in the dynamic state, since the posture angle latch unit 105 is active and latches the posture matrix R _new , the transposition processing unit 106 uses the posture matrix R _new latched by the posture angle latch unit 105 as R _new ^T. constantly updated, it is calculated simply transposed transposed matrix of the current orientation matrix R _new to. That is, since R _new ^T and R _new maintain an orthogonal relationship, I calculated by Equation 5 is a 3 × 3 unit matrix.

On the other hand, in the stationary state, the posture angle latch unit 105 holds the posture matrix R before the transition from the dynamic state to the stationary state, and thus the transposition processing unit 106 transitions from the dynamic state to the stationary state. A transpose matrix ^RT of the previous posture matrix R is calculated. That is, the attitude matrix R _new gradually drifts from the correct attitude due to the gyro offset error and the Kalman filter correction, whereas the ^RT calculated by the transposition processing unit 106 changes from the dynamic state to the stationary state. Since this is a transposed matrix of the posture matrix R before the transition, the error amount for the drift can be calculated by performing the multiplication process of R ^T and R _new (Formula 6).

Here, δR _new is an attitude angle error matrix, and δθ ₁₁ , δθ ₁₂ , δθ ₁₃ , δθ ₂₁ , δθ ₂₂ , δθ ₂₃ , δθ ₃₁ , δθ ₃₂ , and δθ ₃₃ are errors in the respective matrix elements.

The correction control unit 108 corrects the posture angle based on the posture angle error calculated by the posture angle error calculation processing unit 107 and the posture matrix R _new generated by the Kalman filter posture correction processing unit 103. And a correction value to be fed back to the posture angle change amount calculation unit 101. For example, the correction control unit 108 corrects the drift error δR _new calculated by the posture angle error calculation processing unit 107 by PI control based on the generated posture matrix R _new . That is, the correction control unit 108 operates only when stationary. The correction control unit 108 performs feedback control by PI control using Equations 7 and 8 so as to maintain the posture angle when transitioning from the dynamic state to the stationary state.

In Equation 7, _{K P} is a P gain, _{K I} is I gain. diffDCM is feedback value by PI control, is corrected to _{R FB} is fed back to the posture angle variation calculator 101. Here, the feedback gain of the PI control is set to a minute value that cancels the correction of the Kalman filter and the correction of the gyro offset drift. As a result, the posture angle stabilization device 100 can switch to the dynamic state before the effect of the feedback is obtained when switching from the stationary state to the dynamic state. Therefore, more continuous posture angle and speed response output can be performed.

  FIG. 2 is a diagram illustrating an example of a hardware configuration of the posture angle stabilization apparatus 100 according to the embodiment of the present invention. The posture angle stabilization apparatus 100 includes, for example, a CPU (Central Processing Unit) 1010, a bus line 1005, a memory 1050, and an input / output I / F 1040.

  The CPU 1010 is a part that controls the attitude angle stabilization device 100 in an integrated manner, and by reading and executing various programs stored in the memory 1050 as appropriate, the CPU 1010 cooperates with the hardware described above, and performs various types of operations according to the present invention. The function is realized.

  The memory 1050 stores a program that is read and executed as appropriate, and stores various information created by the execution of the program. For example, the memory 1050 stores various calculation results such as a gain value for PI control and a change amount of the posture angle.

  The input / output I / F 1040 includes a posture angle stabilization device 100 that receives an angular velocity (ω) from a high-precision speed measurement device by a three-axis gyro sensor, a posture angle correction value (δθ) from a Kalman filter, and an integrated GPS / IMU speed ( V) is input, and the attitude angle is output to the high-accuracy speed measuring device.

  FIG. 3 is a flowchart showing the processing contents of the posture angle stabilization apparatus 100 according to the embodiment of the present invention.

  In step S101, the CPU 1010 inputs the angular velocity (ω), the attitude angle correction value (δθ) from the Kalman filter, and the integrated velocity (V) from the high-precision velocity measuring device. Thereafter, the CPU 1010 advances the processing to step S102.

  In step S102, the CPU 1010 (posture angle change amount calculation unit 101) calculates the change amount of the posture angle from the input angular velocity (ω) and the feedback correction value. Thereafter, the CPU 1010 advances the processing to step S103.

  In step S103, the CPU 1010 (integration processing unit 102) integrates the change amount of the posture angle calculated in step S102. Thereafter, the CPU 1010 advances the processing to step S104.

  In step S104, the CPU 1010 (Kalman filter posture correction processing unit 103) generates a posture matrix from the posture angle integrated in step S103 and the posture angle correction value (δθ) from the Kalman filter, and outputs the posture matrix to the high-accuracy speed measurement device. To do. Thereafter, the CPU 1010 advances the processing to step S105.

  In step S105, the CPU 1010 (speed turning determination unit 104) determines whether or not it is in a stationary state. More specifically, the CPU 1010 determines whether or not the input integration speed (V) is less than a certain threshold value and the input angular velocity (ω) is less than a certain threshold value. If this determination is YES, the CPU 1010 moves the process to step S106, and if NO, the CPU 1010 moves the process to step S107.

In step S106, CPU 1010 (attitude angle latch portion 105) without storing the generated posture matrix _{R new} in step S104, to hold the memory of previous posture matrix that is determined to be less than the threshold value. Thereafter, the CPU 1010 advances the processing to step S108.

In step S107, the CPU 1010 (attitude angle latch unit 105) stores the attitude matrix R _new generated in step S104. Thereafter, the CPU 1010 advances the processing to step S108.

In step S108, the CPU 1010 (transposition processing unit 106) calculates a transpose matrix R _new ^T of the posture matrix stored in step S106 or step S107. Thereafter, the CPU 1010 advances the processing to step S109.

In step S109, the CPU 1010 (attitude angle error calculation processing unit 107) performs a multiplication process of the transposed matrix R _new ^T calculated in step S108 and the attitude matrix R _new generated in step S104, and calculates the attitude angle error. calculate. Thereafter, the CPU 1010 advances the processing to step S110.

In step S110, CPU 1010 (correction control unit 108) calculates a posture angle error calculated in the step S109, the correction control value for correcting the posture angle based on the posture matrix _{R new} generated in step S104 The correction value is fed back to step S102. Thereafter, the CPU 1010 shifts the processing to step S101.

  FIG. 4 is a diagram illustrating a change in posture angle output in the posture angle stabilization apparatus 100 according to an embodiment of the present invention. FIG. 4A shows a change in the speed of the moving body and shows the time of the stationary state. FIG. 4B is a diagram showing a change in posture angle calculated in the prior art. FIG. 4C is a diagram illustrating a change in posture angle calculated by the posture angle stabilization apparatus 100.

  4B, it is shown that the angle drift of the posture angle is gradually increased. On the other hand, in the stationary state of FIG. 4 (c), the attitude angle stabilizing device 100 eliminates the fact that the angular drift when the moving body is stationary is eliminated, and the angular drift is small and constant width even in the stationary state. And stable.

  According to the present embodiment, the posture angle stabilization apparatus 100 calculates a posture angle change amount calculation unit that calculates a change amount of the posture angle from the angular velocity of the moving body measured using the IMU and the feedback correction value. 101, an integration processing unit 102 that integrates the change amount of the posture angle calculated by the posture angle change amount calculation unit 101, a posture angle integrated by the integration processing unit 102, and an estimated value of the posture angle estimated by the Kalman filter The Kalman filter attitude correction processing unit 103 that generates and outputs the attitude matrix and the velocity measured by the GPS receiver is less than a certain threshold, and the angular velocity measured using the IMU is less than the certain threshold When the speed turning determination unit 104 and the speed turning determination unit 104 determine whether the speed or the angular speed is equal to or greater than a certain threshold, When the attitude matrix generated by the Le Mans filter attitude correction processing unit 103 is latched and it is determined that the velocity and the angular velocity are less than a certain threshold, the generated attitude matrix is not latched and is less than the threshold. Calculated by the transposition processing unit 106, the transposition processing unit 106 that calculates the transpose matrix of the attitude matrix latched by the attitude angle latch unit 105, and the transposition processing unit 106. Multiplying the transposed matrix and the posture matrix generated by the Kalman filter posture correction processing unit 103 to calculate a posture angle error, and the posture angle error calculation processing unit 107 calculate the posture angle error. The posture angle is corrected based on the posture angle error and the posture matrix generated by the Kalman filter posture correction processing unit 103. The correction control value is calculated for, it includes a correction control unit 108 to the correction value is fed back to the posture angle change calculation unit 101, a. Therefore, the attitude angle stabilization device 100 is incorporated into a high-accuracy speed measurement device that uses a Kalman filter using a GPS receiver, and eliminates changes in the position, speed, and direction of the moving object when stationary, Even when switching from a stationary state to a dynamic state, continuous posture angle and speed response output can be performed, and as a result, measurement accuracy at low speeds and low turns can be improved. It is.

  As mentioned above, although embodiment of this invention was described, this invention is not restricted to embodiment mentioned above. The effects described in the embodiments of the present invention are only the most preferable effects resulting from the present invention, and the effects of the present invention are limited to those described in the embodiments of the present invention. is not.

DESCRIPTION OF SYMBOLS 100 Attitude angle stabilization apparatus 101 Attitude angle variation calculation part 102 Integration process part 103 Kalman filter attitude | position correction process part 104 Speed turning determination part 105 Attitude angle latch part 106 Transposition process part 107 Attitude angle error calculation process part 108 Correction control part

Claims (2)

Included in a device that estimates the speed of a moving object using a Kalman filter from the measured speed measured using a GPS receiver and IMU, and calculates the speed of the moving object using an autonomous navigation algorithm by combining the estimated speed and the measured speed. A posture angle stabilization device,
Posture angle change amount calculating means for calculating a change amount of the posture angle from the angular velocity of the moving body measured using the IMU and the feedback correction value;
Integration processing means for integrating the posture angle change amount calculated by the posture angle change amount calculating means;
A Kalman filter posture correction processing unit that generates and outputs a posture matrix from the posture angle integrated by the integration processing unit and the estimated value of the posture angle estimated by the Kalman filter;
Speed turning determination means for determining whether the speed measured by the GPS receiver is less than a certain threshold and the angular velocity measured using the IMU is less than a certain threshold;
When it is determined by the speed turning determination means that the speed or angular velocity is equal to or greater than a certain threshold value, the attitude matrix generated by the Kalman filter attitude correction processing means is latched, and the velocity and angular velocity are less than the certain threshold value. A posture angle latch means for holding a latch of a posture matrix before it is determined to be less than the threshold without latching the generated posture matrix when it is determined to be;
A transposition processing means for calculating a transpose matrix of a posture matrix latched by the posture angle latch means;
Posture angle error calculation processing means for performing a multiplication process of the transposition matrix calculated by the transposition processing means and the posture matrix generated by the Kalman filter posture correction processing means, and calculating a posture angle error;
Calculating a correction control value for correcting a posture angle based on the posture angle error calculated by the posture angle error calculation processing unit and the posture matrix generated by the Kalman filter posture correction processing unit; Correction control means for the correction value to be fed back to the amount calculation means;
A posture angle stabilization device comprising: Included in a device that estimates the speed of a moving object using a Kalman filter from the measured speed measured using a GPS receiver and IMU, and calculates the speed of the moving object using an autonomous navigation algorithm by combining the estimated speed and the measured speed. A method performed by a posture angle stabilization device,
A posture angle change amount calculating step of calculating a change amount of the posture angle from the angular velocity of the moving body measured using the IMU and the feedback correction value;
An integration processing step of integrating the posture angle change amount calculated by the posture angle change amount calculating step;
A Kalman filter posture correction processing step of generating and outputting a posture matrix from the posture angle integrated by the integration processing step and the estimated value of the posture angle estimated by the Kalman filter;
A speed turning determination step for determining whether the speed measured by the GPS receiver is less than a certain threshold value and the angular velocity measured using the IMU is less than a certain threshold value;
When the speed / turning determination step determines that the speed or angular velocity is equal to or greater than a certain threshold, the posture matrix generated by the Kalman filter posture correction processing step is latched, and the velocity and angular velocity are less than the certain threshold. A posture angle latching step for holding a latch of a posture matrix before it is determined to be less than the threshold without latching the generated posture matrix when it is determined to be;
A transposition processing step of calculating a transpose matrix of the posture matrix latched by the posture angle latch step;
A posture angle error calculation processing step of multiplying the transposition matrix calculated by the transposition processing step and the posture matrix generated by the Kalman filter posture correction processing step to calculate a posture angle error;
Calculating a correction control value for correcting a posture angle based on the posture angle error calculated by the posture angle error calculation processing step and the posture matrix generated by the Kalman filter posture correction processing step; A correction control step for the correction value to be fed back to the amount calculation step;
A method comprising:

Images