WO2020245902A1 - 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム - Google Patents

方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム Download PDF

Info

Publication number
WO2020245902A1
WO2020245902A1 PCT/JP2019/022131 JP2019022131W WO2020245902A1 WO 2020245902 A1 WO2020245902 A1 WO 2020245902A1 JP 2019022131 W JP2019022131 W JP 2019022131W WO 2020245902 A1 WO2020245902 A1 WO 2020245902A1
Authority
WO
WIPO (PCT)
Prior art keywords
covariance matrix
value
observed
unit
noise covariance
Prior art date
Application number
PCT/JP2019/022131
Other languages
English (en)
French (fr)
Inventor
服部　和生
光伯 齊藤
百合夏 金井
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to PCT/JP2019/022131 priority Critical patent/WO2020245902A1/ja
Priority to JP2021524532A priority patent/JP7066062B2/ja
Publication of WO2020245902A1 publication Critical patent/WO2020245902A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
    • G01C19/56—Turn-sensitive devices using vibrating masses, e.g. vibratory angular rate sensors based on Coriolis forces
    • G01C19/567—Turn-sensitive devices using vibrating masses, e.g. vibratory angular rate sensors based on Coriolis forces using the phase shift of a vibration node or antinode
    • G01C19/5691—Turn-sensitive devices using vibrating masses, e.g. vibratory angular rate sensors based on Coriolis forces using the phase shift of a vibration node or antinode of essentially three-dimensional vibrators, e.g. wine glass-type vibrators
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration

Definitions

  • the present invention relates to an azimuth estimation device, a gyro system, an azimuth estimation method and a program for estimating an azimuth angle using a resonator of a circular object.
  • An azimuth estimation device that uses a resonator of a circular object is used to detect or control the attitude of a moving object such as a spacecraft.
  • a hemispherical resonance type gyro using a hemispherical resonator is a vibration rotation sensor including a hemispherical resonator, a forcer, and a pick-off (for example, Patent Document 1).
  • the hemispherical resonator is firstly resonated by a forcer to excite the elliptical deformation mode, and the vibration shape of the hemispherical resonator is measured by pick-off.
  • the azimuth angle which is the wave front of the resonance of the hemispherical resonator, changes. Therefore, the angular velocity is detected by calculating the change from the pick-off output.
  • the hemispherical resonance type gyro of Patent Document 1 detects the vibration of the hemispherical resonator from eight displacement sensors arranged at equal intervals around the resonator, and derives the wave antinode azimuth. Specifically, the 0-degree component signal calculated from the four sensor outputs arranged at 90-degree intervals from the reference 0-degree position and the four sensor outputs arranged at 90-degree intervals from the 45-degree position. The wave antinode angle is derived by obtaining the amplitude ratio of the 45-degree component signal calculated from.
  • the 0 degree component signal and the 45 degree component signal are once converted into a pulse train and then input to the integrator. At this time, the number of pulse trains having a small amplitude and the number of pulse trains having a large amplitude are changed to adjust the integrator to be in a balanced state. It is explained that the wave antinode angle can be derived from the ratio of the number of input pulses at the end of an arbitrary sampling cycle.
  • the detection accuracy of the angular velocity of the hemispherical resonance type gyro depends on the measurement accuracy of the wave antinode angle, it is important to improve the measurement accuracy.
  • the sensor output signal contains noise, which causes a decrease in measurement accuracy.
  • the present invention has been made in view of the above circumstances, and provides an azimuth estimation device, a gyro system, an azimuth estimation method and a program having high azimuth estimation accuracy and good azimuth output response performance. The purpose.
  • the azimuth angle estimation device of the present invention is a sensor that acquires a sensor output value indicating displacement from a plurality of sensors arranged on a circular object whose resonance mode is excited by an AC drive signal of an actuator.
  • the wave antinode angle estimation value is estimated based on the observation equation and the state equation using the observation value of the vibration amplitude and the observation value of the resonance phase characteristic derived by the vibration shape derivation unit and the time change of the sensor output value. It is characterized by including a state estimation unit.
  • the accuracy of the model can be improved by using the observed values of the vibration amplitude and the resonance phase characteristic calculated based on the sensor output value in the calculation of the observation equation and the state equation. It is possible to improve the estimation accuracy and improve the response performance of the azimuth output.
  • Block diagram showing the overall configuration of the gyro system according to the first embodiment of the present invention The figure which showed the processing algorithm of the state estimation part which concerns on Embodiment 1.
  • Flowchart of wave azimuth estimation process according to the first embodiment A block diagram showing an overall configuration of a gyro system according to a second embodiment of the present invention.
  • FIG. 1 is a block diagram showing an overall configuration of the gyro system 1 according to the first embodiment.
  • the gyro system 1 according to the first embodiment estimates the azimuth angle based on the outputs of the circular object 10 which is a resonator and the plurality of sensors 11 attached to the circular object 10.
  • the azimuth angle estimation device 20 is provided.
  • the circular object 10 is a resonator having a shape including a circle of an arbitrary size, and has, for example, a hemispherical shape.
  • a circular object 10 having a hemispherical shape is used in the first embodiment.
  • a plurality of sensors 11 are arranged at equal intervals along the edge of the hemisphere.
  • the sensor 11 is an arbitrary displacement sensor.
  • Em 1, 2, ... ⁇
  • m is a serial number assigned to the plurality of sensors 11.
  • Azimuth estimation device 20 includes an arithmetic unit 21 and the storage unit 22 executes the arithmetic processing including antinodes azimuth estimating process based on the sensor output value E m.
  • the arithmetic unit 21 is composed of, for example, a CPU (Central Processing Unit), a ROM (Read Only Memory), and a RAM (Random Access Memory). By executing the program stored in the storage unit 22, the calculation unit 21 functions as a sensor output acquisition unit 211, a vibration shape derivation unit 212, and a state estimation unit 213, as shown in FIG.
  • Sensor output acquisition unit 211 acquires the sensor output value is input from a plurality of sensors 11 E m, and outputs the vibration shape deriving unit 212.
  • E Rek is the real part of the time variation of the sensor output value E m
  • E imk is the imaginary part of the time variation of the sensor output value E m
  • k is a serial number assigned to the time arranged at a predetermined time interval.
  • Vibrating shape deriving unit 212 the observed value of the vibration amplitude A of the primary resonance mode shown a vibration shape from the sensor output value E m, and the observed value of the resonance phase characteristics phi r of the first-order resonance mode for AC drive signal of the actuator Is calculated. Observed value and the observed value of the resonance phase characteristics phi r of the vibration amplitude A is output to the state estimation unit 213.
  • the specific processing in the sensor output acquisition unit 211, the vibration shape derivation unit 212, and the state estimation unit 213 will be described when the circular object 10 is a hemispherical resonance type gyro which is a hemispherical resonator.
  • the hemispherical resonance type gyro is input by first-order resonating a hemispherical resonator, which is a circular object 10, by an AC drive signal of an actuator, and detecting a change in the wave antinode angle of the resonance mode with a plurality of sensors 11. Detects angular velocity.
  • the AC drive signal of the actuator refers to a sine wave / cosine wave drive signal of the actuator that resonates the hemispherical resonator.
  • ⁇ r is the drive angular frequency of the actuator, which is the primary resonance frequency of the hemispherical resonator or a value close to the primary resonance frequency of the hemispherical resonator.
  • the wave node orthogonal phase vibration is 90 degrees with respect to the primary resonance mode when the wave node of the primary resonance mode is made a new wave front in a real hemispherical resonator having non-uniformity in density and rigidity. Refers to the vibration generated by the phase difference.
  • the low frequency components E DC + and E DC- are obtained from the positive rotating coordinate system expression E + and the negative rotating coordinate system expression E ⁇ given by the equation (4) by a low frequency region transmission filter or the like.
  • the low frequency components E DC + and E DC- are expressed by Eq. (5).
  • Equation (5) E DC +, from E DC-, the first resonance mode excited in a hemispherical resonator, the resonance phase characteristics phi r for AC drive signal of the actuator, obtained from equation (6) Be done.
  • the resonance phase characteristic ⁇ r can be obtained.
  • E a and E b represented by the equation (11) are calculated.
  • the vibration shape deriving unit 212 derives the resonance phase characteristic ⁇ r indicating the vibration shape in the primary resonance mode, the wave antinode angle ⁇ r in the primary resonance mode, and the vibration amplitude A.
  • the derived values are passed to the state estimation unit 213 as observed values of the resonance phase characteristic ⁇ r , the wave antinode angle ⁇ r in the primary resonance mode, and the vibration amplitude A.
  • the Kalman filter is a filter that sequentially estimates the true state based on the target model under the assumption that the noise follows a Gaussian distribution with respect to the observed value on which the noise is superimposed.
  • Equation (14) is called the equation of state.
  • the observation model that maps the state space to the observation space is h k
  • the observation noise superimposed on the observed value is v k
  • Equation (15) is called the observation equation.
  • the hemispherical resonance type gyro controls the drive of the actuator based on the observed vibration shape of the hemispherical resonator to keep the vibration shape of the hemispherical resonator constant.
  • the vibration state of the resonator is in the steady state shown in the equation (16).
  • Equation (18) representing the observed value of the vibration amplitude A and the resonance phase characteristics phi r deflection shape deriving unit 212 is calculated using Equation (6) and (13) as in equation (18).
  • the observation equation of the discrete-time nonlinear probability system at time k can be written as in the equation (19).
  • This observation equation represents the temporal and spatial deformation of the resonating circular object 10.
  • the real part cos (2 ⁇ rk ) and the imaginary part sin (2 ⁇ rk ) represent the spatial deformation of the circular object 10.
  • the cos ( ⁇ r kT) of the real part and the imaginary part represents the temporal deformation of the circular object 10.
  • Equation (19) ⁇ rk can be written as in equation (20).
  • FIG. 2 is a diagram showing a processing algorithm of the state estimation unit 213.
  • the estimated value of the target state quantity at time k and the estimated value of the error covariance matrix are expressed as in Eq. (26).
  • the state estimation unit 213 calculates the Kalman gain K k at time k.
  • Kalman gain K k is expressed by equation (29).
  • R k is an observed noise covariance matrix
  • H k is expressed by Eq. (30).
  • h k is a matrix represented by the equation (31).
  • the vibration shape deriving unit 212 substitutes the observed values of A and ⁇ r calculated using the equations (6) and (13) into the equation (31) to calculate h k (step S101).
  • Equation (30) is calculated as the equation (32).
  • a vibration shape deriving unit 212 is calculated using Equation (6) and (13), H k obtained by substituting an observed value of phi r is determined (step S102). Further, the Kalman gain K k is calculated using the equation (29) (step S103).
  • the state estimation unit 213, E shows the sensor output acquisition unit 211 the time variation of the sensor output value E m Rek, by substituting the equation (19) receives the E imk, calculates the observed value y k (Step S104). Further, the estimated value of the state quantity at the time k shown in the equation (33) is calculated by using the observed value y k and the Kalman gain K k (step S105).
  • the estimated value of the error covariance matrix at the time k is calculated by the equation (34) (step S106), and the predicted value of the state quantity is calculated by the equation (35). It is calculated (step S107), and the predicted value of the error covariance matrix is calculated by the equation (36) (step S108).
  • Q k is a system noise covariance matrix
  • R k a value relatively small with respect to the observed noise covariance matrix
  • F k is represented by the equation (37).
  • the system noise covariance matrix Q k is relatively small with respect to the observed noise covariance matrix R k
  • the values of each element of the system noise covariance matrix Q k correspond to the observed noise covariance matrix. The case where it is relatively small with respect to the value of each element of R k is shown. Which element of the matrix is relatively small depends on what is being estimated.
  • the state estimation unit 213 updates the time from k to k + 1 (step S109), and shifts to the next calculation cycle. Based on these equations, the estimated value of the state quantity at each time is calculated (step S105), the calculation of the estimated value of the wave flank azimuth of the equation (38) is repeated (step S110), and finally, The estimated value of the wave flank azimuth shown in the equation (39) can be calculated.
  • FIG. 3 is a flowchart showing the wave antinode angle estimation process executed by the calculation unit 21.
  • the sensor output acquisition unit 211 of the operation unit 21 of the azimuth estimation device 20 is provided in a circle object 10 is output (step S202).
  • Sensor output acquisition unit 211 the sensor output value E m outputted to the vibration shape deriving unit 212, E shows the time change of the sensor output Rek, and outputs the state estimation unit 213 calculates the E imk.
  • the vibration shape derivation unit 212 derives the deflection shape. Specifically, using equations (1) to (13), the vibration shape deriving unit 212 refers to the observed value of the vibration amplitude A in the primary resonance mode, which is a value indicating the vibration shape, and the AC drive signal of the actuator. the observed value of the resonance phase characteristics phi r of the primary resonance mode, to calculate the (step S203). The observed value shown in the equation (18) is output to the state estimation unit 213.
  • the state estimation unit 213 indicates the temporal change of the sensor output which is input from the sensor output acquisition unit 211, and E imk, the information of the vibration shapes received from the vibration shape deriving unit 212, circular
  • the state of the object 10 is estimated, and the estimated value of the wave antinode angle ⁇ r is calculated.
  • step S208: No the calculation unit 21 of the azimuth angle estimation device 20 determines the end of the operation, and if there is no end instruction (step S208: No), the process returns to step S201 to move to the next calculation cycle, and when the end instruction is given. (Step S208: Yes) ends the process.
  • the azimuth angle estimation device 20 of the gyro system 1 has a vibration shape deriving unit 212 based on the sensor output value Em acquired by the sensor output acquisition unit 211 from the sensor 11. There calculates the observed value of the vibration amplitude a of the primary resonance mode, the observed value of the resonance phase characteristics phi r of the first-order resonance mode for AC drive signal of the actuator. Then, the state estimation unit 213 estimates the wave flank azimuth ⁇ r by substituting the observed value of the vibration amplitude A and the observed value of the resonance phase characteristic ⁇ r into the observation equation. Accordingly, in comparison with a method to assign a fixed value of the vibration to the conventional observation equation amplitude A and the resonance phase characteristics phi r, increased model accuracy, the more accurately can be estimated azimuth.
  • FIG. 4 is a block diagram showing an overall configuration of the gyro system 2 according to the second embodiment.
  • the gyro system 2 according to the second embodiment estimates the azimuth angle based on the output of the circular object 10 which is a resonator and the outputs of the plurality of sensors 11 attached to the circular object 10.
  • the azimuth angle estimation device 20 is provided.
  • the configuration of the circular object 10 and the sensor 11 is the same as that of the first embodiment.
  • Azimuth estimating apparatus 20 includes a calculation unit 21 and the storage unit 22 executes the arithmetic processing including azimuth estimation processing based on the sensor output value E m.
  • the calculation unit 21 includes the sensor output acquisition unit 211, the vibration shape derivation unit 212, the state estimation unit 213, and the switching determination unit 214, as shown in FIG. It functions as a variance matrix adjusting unit 215.
  • the configuration of the sensor output acquisition unit 211 and the vibration shape derivation unit 212 is the same as that of the first embodiment.
  • the vibration shape deriving unit 212 is an observed value indicating the vibration shape, and is a resonance phase characteristic ⁇ of the primary resonance mode with respect to the observed value of the vibration amplitude A of the primary resonance mode shown in the equation (18) and the AC drive signal of the actuator.
  • the observed value of r is calculated using the equations (6) and (13) and passed to the state estimation unit 213.
  • the vibration shape deriving unit 212 is a observed value indicating the vibration shape, the observed value antinode azimuth theta r shown in equation (40), is calculated using equation (12), the switching determination unit 214 Pass to.
  • the vibration shape derivation unit 212 calculates equation (40) antinode azimuth theta r shown in observations and state estimating unit 213 has calculated the formula to antinode azimuth theta r indicating (39)
  • the switching determination is performed based on the estimated value, and the switching determination result is sent to the covariance matrix adjustment unit 215 and the state estimation unit 213.
  • the covariance matrix adjusting unit 215 selects or calculates an appropriate covariance matrix according to the conditions, and the state estimation unit 213 uses the covariance matrix to wave. recalculate the estimated value of abdominal azimuth theta r, and outputs the estimated value of the obtained anti-nodes azimuth theta r as the final value.
  • the processes in the sensor output acquisition unit 211, the vibration shape derivation unit 212, and the state estimation unit 213 include the processes described in the first embodiment, the description of the overlapping processes will be omitted.
  • the specific processing of the state estimation unit 213, the switching determination unit 214, and the covariance matrix adjustment unit 215 will be described when the circular object 10 is a hemispherical resonance type gyro which is a hemispherical resonator.
  • Vibrating shape derivation unit 212 based on the sensor output value E m, performs the same calculation as in the first embodiment, by using the equation (12), the observed value antinode azimuth theta r shown in equation (40) Is calculated.
  • the state estimation unit 213, the time change E Rek sensor output value E m, based on the E imk information indicating a vibration shape received from the vibration shape deriving unit 212 performs the same calculation as in the first embodiment, The estimated value of the wave antinode angle ⁇ r shown in the equation (39) is calculated.
  • FIG. 5 is a diagram showing a processing algorithm of the switching determination unit 214.
  • the switching determination unit 214 determines the observed value of the wave-bellied azimuth at time k received from the vibration shape deriving unit 212 and the wave-bellied azimuth at time k received from the state estimation unit 213.
  • the switching determination flag is changed to 1.
  • the switching determination flag is set to 0. That is, the switching determination flag is set as in the equation (41).
  • the switching determination unit 214 sends the switching determination flag determined in this way to the covariance matrix adjustment unit 215 and the state estimation unit 213.
  • FIG. 6 is a diagram showing a processing algorithm of the covariance matrix adjustment unit 215.
  • the switching judgment flag is 1 in the conditional branch of the equation (41)
  • the difference between the observed value and the estimated value of the wave antinode angle ⁇ r is large, and the estimated value does not follow the observed value in the current filter characteristics. Can be judged. Therefore, in order to improve the followability of the Kalman filter, the system noise covariance matrix Q k is made relatively large with respect to the observed noise covariance matrix R k .
  • the system noise covariance matrix Q k when the system noise covariance matrix Q k is made relatively large with respect to the observed noise covariance matrix R k , the values of each element of the system noise covariance matrix Q k are set to the corresponding observed noise covariance matrix R. It is made relatively large with respect to the value of each element of k . Which element of the matrix is relatively large depends on what is being estimated.
  • the switching determination flag is 0 (step S301: No)
  • the covariance matrix adjusting unit 215 ends the calculation without performing processing (step S303).
  • FIG. 7 is a diagram showing a processing algorithm of the state estimation unit 213.
  • the process included in the first step is the same as the process of the state estimation unit 213 according to the first embodiment, and thus the description thereof will be omitted.
  • step S111 when the switching determination flag is 1 in the second step (step S111: Yes), the system noise covariance matrix Q'received from the covariance matrix adjustment unit 215 is used to time k-.
  • the predicted value of the error covariance matrix in 1 is calculated using the equation (42) (step S112).
  • equations (29), (33) to (36) are recalculated using the predicted value of the error covariance matrix obtained by the equation (42) (steps S113 to 117). Then, from the estimated value of the state amount at time k calculated in step S114 to calculate an estimate antinode azimuth theta r (step S118) and outputs.
  • step S111 when the switching determination flag received by the state estimation unit 213 is 0 (step S111: No), the wave flank azimuth angle is derived from the estimated value of the state quantity derived in step S105 in the first step without recalculation. The estimated value of ⁇ r is derived and output. After all the calculations at time k are completed, the state estimation unit 213 updates the time from k to k + 1 and shifts to the next calculation cycle. At this time, regarding the update of the estimated value of the state quantity calculated in steps S116 and 117 and the estimated value of the error covariance matrix, if the switching determination flag is 1, the updated value is updated.
  • FIG. 8 is a flowchart showing the wave antinode angle estimation process executed by the calculation unit 21.
  • the sensor output acquisition unit 211 of the operation unit 21 of the azimuth estimating device 20 obtains the sensor output value E m to sensor 11 provided in the circular object 10 is output (step S202).
  • Sensor output acquisition unit 211 the sensor output value E m outputted to the vibration shape deriving unit 212, E shows the time change of the sensor output Rek, and outputs the state estimation unit 213 calculates the E imk.
  • the vibration shape derivation unit 212 derives the deflection shape (step S203). Specifically, using equations (1) to (13), the vibration shape deriving unit 212 observes the vibration amplitude A in the first-order resonance mode, which is a value indicating the vibration shape, and the wave in the first-order resonance mode. calculates the observed value of the abdominal azimuth theta r, the observed value of the resonance phase characteristics phi r of the primary resonance mode for AC drive signal of the actuator, the. The observed value shown in the equation (18) is output to the state estimation unit 213, and the observed value shown in the equation (40) is output to the switching determination unit 214.
  • the vibration shape deriving unit 212 observes the vibration amplitude A in the first-order resonance mode, which is a value indicating the vibration shape, and the wave in the first-order resonance mode. calculates the observed value of the abdominal azimuth theta r, the observed value of the resonance phase characteristics phi r of the primary resonance mode for AC drive signal of the actuator, the
  • E state estimation unit 213 indicates the temporal change of the sensor output value E m input from the sensor output acquisition unit 211 Rek, and E imk, the information of the vibration shapes received from the vibration shape deriving unit 212 based on , The state of the circular object 10 is estimated, and the estimated value of the wave antinode angle ⁇ r is calculated.
  • the switching determination unit 214 determines whether or not switching is necessary.
  • the value of the switching determination flag is set based on the equation (41).
  • the absolute value of the difference between the observed value of the wave-bellied azimuth output by the vibration shape deriving unit 212 and the estimated value of the wave-bellied azimuth output by the state estimation unit 213 is smaller than the threshold value, and the switching judgment flag is 0. If it is (step S205: No), it ends the calculation and processing estimates antinode azimuth theta r from the estimated value of the state quantity at that time without recalculation.
  • the covariance matrix adjusting unit 215 changes the system noise covariance matrix (step S206) and passes the covariance matrix to the state estimation unit 213.
  • the state estimation unit 213 recalculates the estimated value antinode azimuth theta r using the system noise covariance matrix received from the covariance matrix adjustment unit 215 (step S207).
  • the calculation unit 21 determines the end of the operation, and if there is no end instruction (step S208: No), the process returns to step S201 to move to the next calculation cycle, and if there is an end instruction (step S208: Yes). ) End the process.
  • the estimated value of the antinodes azimuth theta r in accordance with the algorithm of the second embodiment, when the absolute value of the difference between the observed values and the estimated values antinode azimuth theta r is large enhanced followability
  • the estimated value can be made to follow the observed value, and when the absolute value of the difference between the observed value and the estimated value of the wave-bellied azimuth ⁇ r is small. the can reduce noise estimates antinode azimuth theta r by the Kalman filter with improved noise immunity.
  • the configuration of the second embodiment an estimate antinode azimuth theta r well estimated with high precision, in response to changes in the observed values, it is possible to change the response and noise resistance It can be said that it has a filter.
  • a filter exerts a great effect on a device in which the change in the wave antinode angle when the angular velocity input is detected is sufficiently large with respect to the magnitude of noise.
  • it is effective for a hemispherical resonance type gyro.
  • the noise resistance is increased to reduce the noise
  • the followability can be improved in order to accurately detect the input.
  • the azimuth angle estimating apparatus 20 of the gyro system 2 the vibration shape deriving unit 212, observed value antinode azimuth theta r on the basis of the sensor output value E m calculated by the passing the switching determination unit 214, the state estimation unit 213, switching determination unit 214 calculates the estimated value antinode azimuth theta r based on the time change information of the vibration shapes of the sensor output value E m Pass to.
  • the switching determination unit 214 determines that switching is necessary when the absolute value of the difference between the observed value and the estimated value of the wave antinode angle ⁇ r is large, and the covariance matrix adjusting unit 215 determines that the system noise covariance matrix Q k.
  • state estimating unit 213 recalculates the estimated value antinode azimuth theta r by using the system noise covariance matrix Q k.
  • the switching is determined to be unnecessary, it was decided to calculate the estimated value antinode azimuth theta r from the estimated value of the state amount before recalculation.
  • Embodiment 3 The gyro system 2 according to the third embodiment will be described with reference to FIGS. 6-9.
  • the overall configuration of the gyro system 2 and the configuration of the azimuth angle estimation device 20 are the same as those of the second embodiment, but the operations of the state estimation unit 213, the switching determination unit 214, and the covariance matrix adjustment unit 215 are different from those of the second embodiment. .. Note that FIGS. 6, 7 and 8 are common to the second embodiment.
  • the system noise covariance matrix Q k is made smaller than the observed noise covariance matrix R k at the stage of step S204 of the flowchart of the wave antinode angle estimation process shown in FIG. Improved noise resistance. Then, the switching determination shown in the equation (41) is performed, and when the absolute value of the difference between the observed value and the estimated value of the wave antinode angle ⁇ rk is equal to or more than the threshold value c, the system noise covariance matrix is performed at the stage of step S206. It was recalculated estimate antinode azimuth theta rk and relatively large for the Q k to measurement noise covariance matrix R k.
  • the system noise covariance matrix Q k is made relatively large with respect to the observed noise covariance matrix R k to improve the followability. Then, when the absolute value of the difference between the observed value and the estimated value of the wave antinode angle ⁇ rk is smaller than the threshold c, the system noise covariance matrix Q k is applied to the observed noise covariance matrix R k at the stage of step S206. The noise resistance is improved by making it relatively small, and the estimated value of the wave antinode angle ⁇ rk is recalculated.
  • the vibration shape deriving unit 212 calculates the observed value antinode azimuth theta r shown in equation (40), the state estimation unit 213 has the formula (39 ), The point of calculating the estimated value of the wave antinode angle ⁇ r is the same as that of the second embodiment. Hereinafter, the points different from the second embodiment will be described.
  • FIG. 9 is a diagram showing a processing algorithm of the switching determination unit 214.
  • Switching determination unit 214 as shown in FIG. 9, the observation value antinode azimuth theta r at time k received from the vibration shape deriving unit 212, an antinode orientation at time k received from the state estimation unit 213 Using the estimated value of the angle ⁇ r , if the absolute value of the difference is smaller than the threshold c, the switching determination flag is changed to 1. On the other hand, when the absolute value of the difference is equal to or greater than the threshold value c, the switching determination flag is set to 0. That is, the switching determination flag is set as in the equation (43).
  • the switching determination unit 214 sends the switching determination flag determined in this way to the covariance matrix adjustment unit 215 and the state estimation unit 213.
  • FIG. 6 is a diagram showing a processing algorithm of the covariance matrix adjustment unit 215.
  • the system noise covariance matrix Q k is made relatively small with respect to the observed noise covariance matrix R k , and the updated system noise covariance matrix Q'is set as the state estimation unit. Pass it to 213.
  • the switching determination flag is 0 (step S301: No)
  • the covariance matrix adjusting unit 215 ends the calculation without performing processing (step S303).
  • the processing algorithm of the state estimation unit 213 is the same as that of the second embodiment.
  • FIG. 8 is a flowchart showing the wave antinode angle estimation process executed by the calculation unit 21.
  • steps S201 to 204 are the same as those in the second embodiment, the description thereof will be omitted.
  • step S205 the switching determination unit 214 makes a switching determination.
  • the value of the switching determination flag is set based on the equation (43).
  • the absolute value of the difference between the observed value of the wave azimuth ⁇ r output by the vibration shape deriving unit 212 and the estimated value of the wave azimuth ⁇ r output by the state estimation unit 213 is equal to or greater than the threshold value and is switched.
  • determination flag may be 0 (step S205: No), and terminates the processing calculating the estimated value antinode azimuth theta r from the estimated value of the state quantity at that time without recalculation.
  • the covariance matrix adjusting unit 215 changes the system noise covariance matrix (step S206) and passes the covariance matrix to the state estimation unit 213.
  • the state estimation unit 213 recalculates the estimated value antinode azimuth theta r using the system noise covariance matrix received from the covariance matrix adjustment unit 215 (step S207).
  • step S208: No the process returns to step S201 to move to the next calculation cycle, and if there is an end instruction (step S208: Yes). ) Stop the operation.
  • the state estimation unit 213 calculates the estimated value of the wave antinode angle ⁇ r
  • the system noise covariance matrix Q k is observed as the noise covariance matrix R k. It should be relatively large.
  • the switching determination unit 214 determines that switching is necessary when the absolute value of the difference between the observed value and the estimated value of the wave antinode angle ⁇ r is small, and determines the system noise covariance matrix Q k as the observed noise covariance matrix R. Recalculate the estimated value of the wave antinode angle ⁇ r by making it relatively small with respect to k .
  • Embodiment 4 The gyro system 2 according to the fourth embodiment will be described with reference to FIGS. 10 and 11.
  • the overall configuration of the gyro system 2 and the configuration of the azimuth angle estimation device 20 are the same as those of the second and third embodiments, but the operations of the state estimation unit 213 and the covariance matrix adjustment unit 215 are different from those of the second and third embodiments.
  • the covariance matrix adjustment unit 215 has been adjusted follow-up property and noise resistance of the Kalman filter by changing the system noise covariance matrix Q k
  • the matrix adjusting unit 215 adjusts the followability and noise resistance of the Kalman filter by changing the observed noise covariance matrix R k .
  • FIG. 10 is a diagram showing a processing algorithm of the covariance matrix adjusting unit 215.
  • the covariance matrix adjusting unit 215 adjusts the observed noise covariance matrix R k when the switching determination flag is 1 (step S401: Yes) (step S402).
  • the updated observation noise covariance matrix R k' is passed to the state estimation unit 213.
  • the switching determination flag is 0 (step S401: No)
  • the covariance matrix adjusting unit 215 ends the calculation without performing processing (step S403).
  • the measurement noise covariance matrix R k By adjusting the measurement noise covariance matrix R k, increase followability of the Kalman filter if the relatively large system noise covariance matrix Q k with respect to the observation noise covariance matrix R k, the measurement noise covariance matrix R k On the other hand, if the system noise covariance matrix Q k becomes relatively small, the noise resistance of the Kalman filter increases. Therefore, the relative magnitude of the observed noise covariance matrix R k with respect to the system noise covariance matrix Q k is changed according to the followability and noise resistance of the ideal filter.
  • FIG. 11 is a diagram showing a processing algorithm of the state estimation unit 213.
  • the process included in the first step is the same as the process of the state estimation unit 213 according to the first embodiment, and thus the description thereof will be omitted.
  • the covariance matrix adjusting unit 215 adjusts the observed noise covariance matrix R k , it is not necessary to calculate the equation (42) in the second and third embodiments, and the observation is adjusted instead.
  • the Kalman gain K k is recalculated using the noise covariance matrix R k '. That is, when the switching determination flag is 1 (step S111: Yes), the Kalman gain K k at the time k is recalculated using the equation (44) (step S113).
  • Equations (33) to (36) are recalculated using the Kalman gain K k obtained by the equation (44) (steps S114 to 117). Then, to derive an estimate antinode azimuth theta r from the estimated value of the state amount at time k calculated in step S114 (step S118) and outputs.
  • step S111 when the switching determination flag received by the state estimation unit 213 is 0 (step S111: No), the wave flank azimuth angle is derived from the estimated value of the state quantity derived in step S105 in the first step without recalculation. The estimated value of ⁇ r is derived and output. After all the calculations at time k are completed, the state estimation unit 213 updates the time from k to k + 1 and shifts to the next calculation cycle. At this time, regarding the update of the estimated value of the state quantity calculated in steps S116 and 117 and the estimated value of the error covariance matrix, if the switching determination flag is 1, the updated value is updated.
  • the operation of the azimuth angle estimation device 20 according to the fourth embodiment described above is the same as that of the second or third embodiment.
  • the state estimation unit 213 changes the observed noise covariance matrix R k when calculating or recalculating the estimated value of the wave antinode angle ⁇ r, and the system It was decided to make it relatively large or small with respect to the noise covariance matrix Q k .
  • the followability and noise resistance of the Kalman filter can be adjusted as in the second and third embodiments, and the calculation of the predicted value of the error covariance matrix can be omitted, so that the calculation processing time can be shortened. Can be done.
  • Embodiment 5 The gyro system 2 according to the fifth embodiment will be described with reference to FIGS. 12 and 13.
  • the overall configuration of the gyro system 2 and the configuration of the azimuth angle estimation device 20 are the same as those of the second embodiment, but the operations of the state estimation unit 213 and the covariance matrix adjustment unit 215 are different from those of the second embodiment.
  • the Kalman filter's followability and noise tolerance are adjusted by changing the system noise covariance matrix Q k or the observed noise covariance matrix R k in the covariance matrix adjusting unit 215.
  • the covariance matrix adjusting unit 215 changes both the system noise covariance matrix Q k and the observed noise covariance matrix R k , as shown in FIG. 12, so that the Kalman filter can follow and withstand. Adjust the noise property.
  • FIG. 12 is a diagram showing a processing algorithm of the covariance matrix adjustment unit 215.
  • the covariance matrix adjusting unit 215 adjusts the system noise covariance matrix Q k and the observed noise covariance matrix R k when the switching determination flag is 1 (step S501: Yes) (step S502). ).
  • the switching determination flag is 0 (step S501: No)
  • the covariance matrix adjusting unit 215 ends the calculation without performing processing (step S503).
  • FIG. 13 is a diagram showing a processing algorithm of the state estimation unit 213.
  • the process included in the first step is the same as the process of the state estimation unit 213 according to the first embodiment, and thus the description thereof will be omitted.
  • the predicted value of the error covariance matrix at time k-1 is recalculated by the adjusted system noise covariance matrix Q k '(step S112), and the Kalman gain K k. Is recalculated with the adjusted observed noise covariance matrix R k '(step S113). Then, the equations (33) to (36) are recalculated (steps S114 to 117). Then, to derive an estimate antinode azimuth theta r from the estimated value of the state amount at time k calculated in step S114 (step S118) and outputs.
  • step S111 when the switching determination flag received by the state estimation unit 213 is 0 (step S111: No), the wave flank azimuth angle is derived from the estimated value of the state quantity derived in step S105 in the first step without recalculation. The estimated value of ⁇ r is derived and output. After all the calculations at time k are completed, the state estimation unit 213 updates the time from k to k + 1 and shifts to the next calculation cycle. At this time, regarding the update of the estimated value of the state quantity calculated in steps S116 and 117 and the estimated value of the error covariance matrix, if the switching determination flag is 1, the updated value is updated.
  • the operation of the azimuth angle estimation device 20 according to the fifth embodiment described above is the same as that of the second embodiment.
  • the state estimation unit 213 calculates or recalculates the estimated value of the wave antinode angle ⁇ r
  • the system noise covariance matrix Q k and the observed noise covariance are It was decided to change the matrix R k to change the relative magnitude of the system noise covariance matrix Q k and the observed noise covariance matrix R k .
  • the followability and noise resistance of the Kalman filter can be adjusted as in the second embodiment.
  • the present invention acquires sensor output values indicating displacement from a plurality of sensors arranged on a circular object whose resonance mode is excited by the AC drive signal of the actuator, and is circular based on the sensor output values.
  • the observed value of the vibration amplitude of the object and the observed value of the resonance phase characteristic of the circular object with respect to the AC drive signal of the actuator are derived.
  • the switching determination unit 214 only one threshold value c as a reference for switching determination is set by the switching determination unit 214, but a plurality of threshold values c are provided to branch the conditions, and each condition is set.
  • the values of the system noise covariance matrix Q k and the observed noise covariance matrix R k may be set. According to this, the ratio of the magnitudes of the system noise covariance matrix Q k and the observed noise covariance matrix R k can be set according to a plurality of existing thresholds c, so that the estimated value with respect to the observed value of the wave antinode angle ⁇ r. It is possible to set a plurality of patterns of followability and noise resistance.
  • a plurality of displacement sensors are used to measure the vibration of the circular object 10, but the vibration is measured by the speed sensor and the obtained data is integrated.
  • the displacement of the vibration may be obtained.
  • the vibration may be measured by an acceleration sensor, and the displacement of the vibration may be obtained by integrating the obtained data in the second order.
  • the Kalman filter is used as the state estimation unit 213, but a filter to which a Kalman filter including an extended Kalman filter, an unsented Kalman filter, or an ensemble Kalman filter is applied may be used as the state estimation unit 213.
  • a filter to which a Kalman filter including an extended Kalman filter, an unsented Kalman filter, or an ensemble Kalman filter is applied may be used as the state estimation unit 213.
  • the uncented Kalman filter it can be applied even when the state quantity has a non-linear state space model.
  • the ensemble Kalman filter it can be applied even when the state quantity has a nonlinear / non-Gaussian state space model.
  • a state estimation means including a particle filter may be used as the state estimation unit 213.
  • a particle filter When a particle filter is used, it can be applied even when the state quantity has a non-linear / non-Gaussian state-space model.
  • the distribution method of such a program is arbitrary, for example, CD-ROM (Compact Disc Read-Only Memory), DVD (Digital Versatile Disc), MO (Magneto Optical Disc), memory card, or other computer readable. It may be stored on a recording medium and distributed, or may be distributed via the Internet or other communication networks.
  • CD-ROM Compact Disc Read-Only Memory
  • DVD Digital Versatile Disc
  • MO Magnetic Optical Disc
  • memory card or other computer readable. It may be stored on a recording medium and distributed, or may be distributed via the Internet or other communication networks.

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Gyroscopes (AREA)

Abstract

方位角推定装置（２０）のセンサ出力取得部（２１１）は、アクチュエータの交流駆動信号により共振モードを励起された円状物体（１０）に配置された複数のセンサ（１１）から、変位を示すセンサ出力値を取得する。振動形状導出部（２１２）は、取得したセンサ出力値に基づいて、円状物体（１０）の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体（１０）の共振位相特性の観測値と、を導出する。状態推定部（２１３）は、振動形状導出部（２１２）が導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を計算する。

Description

方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム
　本発明は、円状物体の共振器を用いて方位角を推定する方位角推定装置、ジャイロシステム、方位角推定方法及びプログラムに関する。
　宇宙機をはじめとする移動体の姿勢検知又は制御に、円状物体の共振器を用いた方位角推定装置が用いられている。半球形状の共振器を用いる半球共振型ジャイロは、半球共振器とフォーサ及びピックオフを備えた振動回転センサである（例えば、特許文献１）。
　半球共振型ジャイロは、フォーサにより半球共振器を１次共振させて楕円変形モードを励起して、ピックオフにより半球共振器の振動形状を計測する。角速度が入力されたとき、半球共振器の共振の波腹となる方位角が変化するため、その変化をピックオフの出力から算出することで、角速度が検出される。
　特許文献１の半球共振型ジャイロは、共振器の周りに等間隔に配置された８つの変位センサから半球共振器の振動を検出し、波腹方位角を導出している。具体的には、基準となる０度の位置から９０度おきに配置された４つのセンサ出力から算出される０度成分信号と、４５度の位置から９０度おきに配置された４つのセンサ出力から算出される４５度成分信号の振幅比を求めることで波腹方位角を導出している。
　この振幅比を求める際に、０度成分信号及び４５度成分信号を一度パルス列に変換してから積分器に入力する。このとき、振幅が小さいパルス列と振幅が大きいパルス列の数を変えて、積分器をつりあい状態にする調整を行う。任意のサンプリングサイクル終了時における入力パルス数の比から波腹方位角を導出することができると説明されている。
特開昭６０－１６６８１８号公報
　半球共振型ジャイロの角速度の検出精度は、波腹方位角の測定精度に依存しているため、その測定精度を向上させることが重要である。波腹方位角の測定において、センサ出力信号には雑音が含まれており、測定精度の低下の原因になっていた。
　ノイズ低減を図るためには、例えば、ローパスフィルタ（Low-Pass Filter：ＬＰＦ）を含む高域成分を除去する信号処理を施すことができる。しかし、このような信号処理を実行する場合、信号処理前後で遅延が生じてしまい、ジャイロの応答性能及び移動体の姿勢制御精度が悪化するという問題があった。
　本発明は上記事情に鑑みてなされたものであり、方位角の推定精度が高く、方位角出力の応答性能が良好な方位角推定装置、ジャイロシステム及び方位角推定方法及びプログラムを提供することを目的とする。
　上記目的を達成するため、本発明の方位角推定装置は、アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得するセンサ出力取得部と、センサ出力値に基づいて、円状物体の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体の共振位相特性の観測値と、を導出する振動形状導出部と、を備える。また、振動形状導出部が導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を推定する状態推定部を備えることを特徴とする。
　本発明によれば、センサ出力値に基づいて算出した振動振幅及び共振位相特性の観測値を、観測方程式及び状態方程式の計算に用いることにより、モデルの精度を高めることができるため、方位角の推定精度を高め、方位角出力の応答性能を良好にすることが可能になる。
本発明の実施の形態１に係るジャイロシステムの全体構成を示すブロック図 実施の形態１に係る状態推定部の処理アルゴリズムを示した図 実施の形態１に係る波腹方位角推定処理のフローチャート 本発明の実施の形態２に係るジャイロシステムの全体構成を示すブロック図 実施の形態２に係る切替判定部の処理アルゴリズムを示した図 実施の形態２，３に係る共分散行列調整部の処理アルゴリズムを示した図 実施の形態２，３に係る状態推定部の処理アルゴリズムを示した図 実施の形態２，３に係る波腹方位角推定処理のフローチャート 本発明の実施の形態３に係る切替判定部の処理アルゴリズムを示した図 本発明の実施の形態４に係る共分散行列調整部の処理アルゴリズムを示した図 実施の形態４に係る状態推定部の処理アルゴリズムを示した図 本発明の実施の形態５に係る共分散行列調整部の処理アルゴリズムを示した図 実施の形態５に係る状態推定部の処理アルゴリズムを示した図
　以下に、本発明を実施するための形態について図面を参照して詳細に説明する。なお、図中同一又は相当する部分には同じ符号を付す。
実施の形態１．
　図１は実施の形態１に係るジャイロシステム１の全体構成を示すブロック図である。本実施の形態１に係るジャイロシステム１は、図１に示すように、共振器である円状物体１０と、円状物体１０に付した複数のセンサ１１の出力に基づいて方位角を推定する方位角推定装置２０と、を備える。
　円状物体１０は、任意の大きさの円を含む形状を有する共振器であり、例えば半球形状を有する。本実施の形態１では、半球形状を有する円状物体１０を用いる場合について説明する。円状物体１０は半球面の縁に沿って、複数のセンサ１１が等間隔で配置されている。
　センサ１１は、任意の変位センサである。円状物体１０に角速度が入力されると、円状物体１０の縁部の振動パターンが変化するため、各センサ１１は、変位量を示すセンサ出力値Ｅｍ（ｍ＝１，２，・・・）を出力する。ここで、ｍは複数のセンサ１１に付与した通し番号である。
　方位角推定装置２０は、センサ出力値Ｅｍに基づいて波腹方位角推定処理を含む演算処理を実行する演算部２１及び記憶部２２を備える。演算部２１は、例えば、ＣＰＵ（Central Processing Unit：中央演算装置）、ＲＯＭ（Read Only Memory：読み出し専用メモリ）、ＲＡＭ（Random Access Memory：ランダムアクセスメモリ）から構成される。演算部２１は、記憶部２２に記憶されているプログラムを実行することにより、図１に示すように、センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３として機能する。
　センサ出力取得部２１１は、複数のセンサ１１から入力されるセンサ出力値Ｅｍを取得して、振動形状導出部２１２に出力する。また、センサ出力取得部２１１は、センサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋ（ｋ＝１，２，・・・）を計算して、状態推定部２１３に出力する。ここで、ＥＲｅｋはセンサ出力値Ｅｍの時間変化量の実数部分であり、ＥＩｍｋはセンサ出力値Ｅｍの時間変化量の虚数部分である。ｋは予め定めた時間間隔で配列した時刻に対して付与した通し番号である。
　振動形状導出部２１２は、センサ出力値Ｅｍから振動形状を示す１次共振モードの振動振幅Ａの観測値、及び、アクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φｒの観測値を算出する。振動振幅Ａの観測値及び共振位相特性φｒの観測値は、状態推定部２１３に対して出力される。
　状態推定部２１３は、センサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、波腹方位角θｒの推定値を算出する。なお、図面において、観測値には文字の上に”~”の記号を付し、推定値には文字の上に”＾”の記号を付している。
　センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３における具体的な処理について、円状物体１０が半球共振器である半球共振型ジャイロの場合について説明する。
　半球共振型ジャイロは、円状物体１０である半球共振器をアクチュエータの交流駆動信号によって１次共振させ、当該共振モードの波腹方位角の変化を複数のセンサ１１で検出することにより入力された角速度を検出する。ここでは、半球共振型ジャイロの最も一般的な構成である、８つの変位センサＳｍ（ｍ＝１，２，・・・，８）を４５度間隔で半球共振器周囲に配置した構成を例として記述する。つまり、変位センサＳｍ（ｍ＝１，２，・・・，８）それぞれがセンサ１１である。
　半球共振器に励起された１次共振モードの変位であって、波節直角位相振動による半径方向変位は、変位センサＳｍ（ｍ＝１，２，・・・，８）によって検出され、このときのセンサ出力値Ｅｍ（ｍ＝１，２，・・・，８）は、式（１）で与えられる。
Figure JPOXMLDOC01-appb-M000001
　ここでアクチュエータの交流駆動信号は、半球共振器を共振させるアクチュエータの正弦波・余弦波駆動信号を指す。ωｒはアクチュエータの駆動角周波数であるが、これが半球共振器の１次共振周波数になり、又は、半球共振器の１次共振周波数に近い値になる。また、波節直角位相振動は、密度および剛性に不均一性を有する現実の半球共振器において、１次共振モードの波節を新たな波腹にしたときの１次共振モードに対して９０度位相差で発生する振動を指す。
　次に、センサ出力値Ｅｍを合成する。具体的には、半球共振器の軸の中心を原点とし、ｍ＝１のセンサが配置されている方位を０度と定義する場合、０，９０，１８０，２７０度の方位角に配置された変位センサＳｍ（ｍ＝１，３，５，７）によるセンサ出力値Ｅｍ（ｍ＝１，３，５，７）、及び、４５，１３５，２２５，３１５度の方位角に配置された変位センサＳｍ（ｍ＝２，４，６，８）によるセンサ出力値Ｅｍ（ｍ＝２，４，６，８）を、式（２）のように合成する。
Figure JPOXMLDOC01-appb-M000002
　ＥＲｅを実数部、ＥＩｍを虚数部とした複素表現は、式（３）のように表される。
Figure JPOXMLDOC01-appb-M000003
　次に、式（３）で与えられる複素表現Ｅを、式（４）を用いて、アクチュエータ駆動周波数ωｒに一致する回転速度で正負方向に回転する。
Figure JPOXMLDOC01-appb-M000004
　次に、式（４）で与えられる正方向回転座標系表現Ｅ＋、及び、負方向回転座標系表現Ｅ－から、低周波数域透過フィルタ等により低周波成分ＥＤＣ＋、ＥＤＣ－を求める。低周波成分ＥＤＣ＋、ＥＤＣ－は式（５）のように表される。
Figure JPOXMLDOC01-appb-M000005
　式（５）で与えられる低周波成分ＥＤＣ＋、ＥＤＣ－から、半球共振器に励起された１次共振モードの、アクチュエータの交流駆動信号に対する共振位相特性φｒが、式（６）より求められる。
Figure JPOXMLDOC01-appb-M000006
　なお、ここでは、実際の１次共振モードおよび波節直角位相振動の振動振幅が、式（７）の関係を満たすことを用いている。
Figure JPOXMLDOC01-appb-M000007
　一方、センサ１１による実測時には、センサ出力取得部２１１が各センサ出力値Ｅｍ（ｍ＝１，２，・・・，８）を取得し、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて、振動形状を導出する。具体的には、まず、振動形状導出部２１２は、センサ出力値Ｅｍの合成値を、式（８）を用いて求める。
Figure JPOXMLDOC01-appb-M000008
　次に、ＥＲｅを実数部、ＥＩｍを虚数部とした、式（９）で示す複素表現を求める。
Figure JPOXMLDOC01-appb-M000009
　次に、式（９）で与えられる複素表現Ｅを、式（１０）を用いてアクチュエータ駆動周波数ωｒに一致する回転速度で正負方向に回転する。正方向回転座標系表現Ｅ＋、及び、負方向回転座標系表現Ｅ－は式（１０）のように表される。
Figure JPOXMLDOC01-appb-M000010
　次に、センサ出力値Ｅｍの実測値から式（１０）を用いて得られた正方向回転座標系表現Ｅ＋、及び、負方向回転座標系表現Ｅ－の低周波成分ＥDC+及びＥDC－を低周波数域透過フィルタ等により算出する。得られた低周波成分ＥDC+及びＥDC－を、理論式である式（６）に代入することにより、共振位相特性φｒが得られる。さらに、式（１１）で示されるＥａ，Ｅｂを算出する。
Figure JPOXMLDOC01-appb-M000011
　次に、Ｅａ，Ｅｂを用いて、半球共振器に励起された１次共振モードの波腹方位角θｒを式（１２）より算出し、振動振幅Ａを式（１３）より算出する。
Figure JPOXMLDOC01-appb-M000012
Figure JPOXMLDOC01-appb-M000013
　このようにして、振動形状導出部２１２は、１次共振モードの振動形状を示す共振位相特性φｒ、１次共振モードの波腹方位角θｒ、振動振幅Ａを導出する。導出された値は共振位相特性φｒ、１次共振モードの波腹方位角θｒ、振動振幅Ａの観測値として状態推定部２１３に受け渡される。
　次に、状態推定部２１３の処理アルゴリズムを説明する。具体的には、半球共振器の１次共振モードの波腹方位角θｒの推定値の導出手法について述べる。ここでは、状態推定部２１３の一例としてカルマンフィルタを適用する場合の処理アルゴリズムを記述する。
　カルマンフィルタは、ノイズが重畳する観測値に対して、ノイズがガウス分布に従うという仮定のもと、対象のモデルに基づいて逐次的に真の状態を推定するフィルタである。ある時刻ｋにおける対象の状態量をｘｋ、一定時刻後の対象の状態量をｘｋ＋１としたとき、ｘｋ＋１は式（１４）で表される。
Figure JPOXMLDOC01-appb-M000014
　式（１４）は状態方程式と呼ばれる。これに対して、ある時刻ｋにおける対象の観測値をｙｋ、状態空間を観測空間に写像する観測モデルをｈｋ、観測値に重畳する観測雑音をｖｋとすると、ｙｋは式（１５）で表される。式（１５）は観測方程式と呼ばれる。
Figure JPOXMLDOC01-appb-M000015
　次に、状態方程式と観測方程式の具体例を示す。半球共振型ジャイロは、観測された半球共振器の振動形状をもとにアクチュエータの駆動を制御して、半球共振器の振動形状を一定に保たせる。ここでは、共振器の振動状態が式（１６）に示す定常状態にある場合を仮定する。
Figure JPOXMLDOC01-appb-M000016
　このとき、式（２）に示すＥＲｅとＥＩｍは式（１７）のように書き直せる。
Figure JPOXMLDOC01-appb-M000017
　ここで、振動形状導出部２１２が式（６）及び（１３）を用いて算出した振動振幅Ａ及び共振位相特性φｒの観測値を式（１８）のように表す。式（１８）に示した観測値を式（１７）に代入することで、時刻ｋにおける離散時間非線形確率システムの観測方程式は、式（１９）のように書き表せる。この観測方程式は、共振する円状物体１０の時間的及び空間的な変形を表している。具体的には、式（１９）において、実数部のｃｏｓ（２θｒｋ）と虚数部のｓｉｎ（２θｒｋ）が円状物体１０の空間的変形を表している。また、式（１９）において、実数部及び虚数部のｃｏｓ（ωｒｋＴ）が円状物体１０の時間的変形を表している。
Figure JPOXMLDOC01-appb-M000018
Figure JPOXMLDOC01-appb-M000019
　ここで、式（１９）においてθｒｋは式（２０）のように書き表せる。
Figure JPOXMLDOC01-appb-M000020
　波腹方位角変動量Δθｒｋは、半球共振型ジャイロ計測軸において式（２１）に示す関係を持つことから、時刻ｋにおける離散時間非線形確率システムの状態方程式は、式（２２）のように書き表せる。
Figure JPOXMLDOC01-appb-M000021
Figure JPOXMLDOC01-appb-M000022
　よって、式（１４）のパラメータを、式（２３）に示すようにおくことにより、式（２４）に示す状態方程式が得られる。
Figure JPOXMLDOC01-appb-M000023
Figure JPOXMLDOC01-appb-M000024
　また、式（１９）及び式（２０）より、式（２５）に示す観測方程式が得られる。
Figure JPOXMLDOC01-appb-M000025
　次に、状態推定部２１３の計算手順について図２を用いて記述する。図２は状態推定部２１３の処理アルゴリズムを示した図である。カルマンフィルタを適用する場合は、時刻ｋにおける対象の状態量の推定値、及び、誤差共分散行列の推定値を式（２６）のように表記する。
Figure JPOXMLDOC01-appb-M000026
　また、時刻における観測値ｙｋをもとに、次の時刻ｋ＋１の状態量を予測した値、及び、誤差共分散行列を予測した値を式（２７）のように表記する。
Figure JPOXMLDOC01-appb-M000027
　まず、状態推定部２１３が初期化を行う。具体的には、状態推定部２１３は時刻ｋ＝０の初期値を式（２８）のように与える。
Figure JPOXMLDOC01-appb-M000028
　これらをもとに、状態推定部２１３は時刻ｋにおけるカルマンゲインＫｋを算出する。カルマンゲインＫｋは式（２９）で表される。
Figure JPOXMLDOC01-appb-M000029
　ここで、Ｒｋは観測雑音共分散行列であり、Ｈｋは式（３０）で表される。
Figure JPOXMLDOC01-appb-M000030
　ここで、ｈｋは式（３１）で表される行列である。振動形状導出部２１２が式（６）及び（１３）を用いて算出したＡ，φｒの観測値を式（３１）に代入してｈｋを計算する（ステップＳ１０１）。
Figure JPOXMLDOC01-appb-M000031
　よって、式（３０）は式（３２）のように計算される。すなわち、振動形状導出部２１２が式（６）及び（１３）を用いて算出したＡ，φｒの観測値を代入したＨｋが求められる（ステップＳ１０２）。また、式（２９）を用いて、カルマンゲインＫｋを計算する（ステップＳ１０３）。
Figure JPOXMLDOC01-appb-M000032
　次に、状態推定部２１３は、センサ出力取得部２１１からセンサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋを受け取って式（１９）に代入することにより、観測値ｙｋを計算する（ステップＳ１０４）。さらに、観測値ｙｋとカルマンゲインＫｋを用いて、式（３３）に示す時刻ｋでの状態量の推定値を算出する（ステップＳ１０５）。
Figure JPOXMLDOC01-appb-M000033
　また、次の時刻ｋ＋１での状態量の計算のため、時刻ｋでの誤差共分散行列の推定値を式（３４）で算出し（ステップＳ１０６）、状態量の予測値を式（３５）で算出し（ステップＳ１０７）、誤差共分散行列の予測値を式（３６）で算出する（ステップＳ１０８）。
Figure JPOXMLDOC01-appb-M000034
Figure JPOXMLDOC01-appb-M000035
Figure JPOXMLDOC01-appb-M000036
　ここで、Ｑｋはシステム雑音共分散行列であり、カルマンフィルタの耐ノイズ性を高めるため、観測雑音共分散行列Ｒｋに対して相対的に小さい値を設定して計算する。また、Ｆｋは、式（３７）で表される。ここで、システム雑音共分散行列Ｑｋが観測雑音共分散行列Ｒｋに対して相対的に小さい場合とは、システム雑音共分散行列Ｑｋの各要素の値が、対応する観測雑音共分散行列Ｒｋの各要素の値に対して相対的に小さい場合を示す。行列のどの要素の値を相対的に小さくするかは、推定する対象に応じて変更する。
Figure JPOXMLDOC01-appb-M000037
　時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して（ステップＳ１０９）、次の計算サイクルに移行する。これらの式をもとに各時刻の状態量の推定値の計算をし（ステップＳ１０５）、式（３８）の波腹方位角の推定値の計算を繰り返して（ステップＳ１１０）、最終的に、式（３９）に示す波腹方位角の推定値を計算することができる。
Figure JPOXMLDOC01-appb-M000038
Figure JPOXMLDOC01-appb-M000039
　以上説明した本実施の形態１の方位角推定装置２０の動作を図３のフローチャートに沿って説明する。図３は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。
　まず、方位角推定装置２０の演算部２１は初期化を行う（ステップＳ２０１）。具体的には、状態推定部２１３に対し、時刻ｋ＝０における初期値を式（２８）のように設定する。
　次に、方位角推定装置２０の演算部２１のセンサ出力取得部２１１が円状物体１０に備えられたセンサ１１が出力するセンサ出力値Ｅｍを取得する（ステップＳ２０２）。センサ出力取得部２１１は、センサ出力値Ｅｍを振動形状導出部２１２に出力し、センサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋを算出して状態推定部２１３に出力する。
　次に、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて、振動形状を導出する。具体的には、式（１）～（１３）を用いて、振動形状導出部２１２が、振動形状を示す値である１次共振モードの振動振幅Ａの観測値と、アクチュエータの交流駆動信号に対する一次共振モードの共振位相特性φｒの観測値と、を計算する（ステップＳ２０３）。式（１８）に示す観測値は、状態推定部２１３に対して出力される。
　次に、状態推定部２１３がセンサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、円状物体１０の状態を推定し、波腹方位角θｒの推定値を計算する。具体的には、式（１４）～（３８）を用いて、センサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状を示す振動振幅Ａと共振位相特性φｒの観測値に基づいて、式（３９）に示す波腹方位角θｒの推定値を計算する（ステップＳ２０４）。
　最後に、方位角推定装置２０の演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）、ステップＳ２０１に戻り次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）、処理を終了する。
　以上説明したように、本実施の形態１によれば、ジャイロシステム１の方位角推定装置２０は、センサ出力取得部２１１がセンサ１１から取得したセンサ出力値Ｅｍに基づいて、振動形状導出部２１２が１次共振モードの振動振幅Ａの観測値と、アクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φｒの観測値を算出する。そして、状態推定部２１３が、振動振幅Ａの観測値及び共振位相特性φｒの観測値を観測方程式に代入することで、波腹方位角θｒを推定することとした。これにより、従来の観測方程式に振動振幅Ａと共振位相特性φｒの固定値を代入する手法に比べて、モデル精度が高まり、より正確に方位角を推定することが可能となる。
実施の形態２．
　図４は実施の形態２に係るジャイロシステム２の全体構成を示すブロック図である。本実施の形態２に係るジャイロシステム２は、図４に示すように、共振器である円状物体１０と、円状物体１０に付した複数のセンサ１１の出力に基づいて方位角を推定する方位角推定装置２０と、を備える。円状物体１０及びセンサ１１の構成は実施の形態１と同様である。
　本実施の形態２に係る方位角推定装置２０は、センサ出力値Ｅｍに基づいて方位角推定処理を含む演算処理を実行する演算部２１及び記憶部２２を備える。演算部２１は、記憶部２２に記憶されているプログラムを実行することにより、図４に示すように、センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３、切替判定部２１４、共分散行列調整部２１５として機能する。センサ出力取得部２１１、振動形状導出部２１２の構成は実施の形態１と同様である。
　センサ出力取得部２１１は、センサ１１のセンサ出力値Ｅｍ（ｍ＝１，２，・・・）を振動形状導出部２１２に、検出されたセンサの出力の時間変化ＥＲｅｋ、ＥＩｍｋを状態推定部２１３に渡す。振動形状導出部２１２は、振動形状を示す観測値であって、式（１８）に示す１次共振モードの振動振幅Ａの観測値及びアクチュエータの交流駆動信号に対する１次共振モードの共振位相特性φｒの観測値を式（６）及び（１３）を用いて算出し、状態推定部２１３に渡す。
　また、振動形状導出部２１２は、振動形状を示す観測値であって、式（４０）に示す波腹方位角θｒの観測値を、式（１２）を用いて算出し、切替判定部２１４に渡す。
Figure JPOXMLDOC01-appb-M000040
　状態推定部２１３は、センサ出力取得部２１１から受け取ったセンサ出力値Ｅｍの時間変化ＥＲｅｋ，ＥＩｍｋと振動形状導出部２１２から受け取った振動形状を示す情報に基づいて、式（３９）に示す波腹方位角θｒの推定値を算出し、切替判定部２１４に渡す。
　切替判定部２１４は、振動形状導出部２１２が算出した式（４０）に示す波腹方位角θｒの観測値と状態推定部２１３が算出した式（３９）に示す波腹方位角θｒの推定値に基づいて切替判定を行い、切替判定結果を共分散行列調整部２１５と状態推定部２１３に送る。
　切替判定の結果、切替が必要と判断された場合は、共分散行列調整部２１５が条件に応じて適切な共分散行列を選択又は算出し、状態推定部２１３が当該共分散行列を用いて波腹方位角θｒの推定値を再計算し、得られた波腹方位角θｒの推定値を最終値として出力する。
　一方、切替判定の結果、切替が不要と判断された場合は、共分散行列調整部２１５は動作せず、最初に状態推定部２１３が算出した波腹方位角θｒの推定値を最終値として出力する。
　センサ出力取得部２１１、振動形状導出部２１２、状態推定部２１３における処理は実施の形態１で述べた処理を含むため、重複する処理については説明を省略する。状態推定部２１３、切替判定部２１４、共分散行列調整部２１５の具体的な処理について、円状物体１０が半球共振器である半球共振型ジャイロの場合について説明する。
　振動形状導出部２１２は、センサ出力値Ｅｍに基づいて、実施の形態１と同様の計算を行い、式（１２）を用いて、式（４０）に示す波腹方位角θｒの観測値を算出する。一方、状態推定部２１３は、センサ出力値Ｅｍの時間変化ＥＲｅｋ，ＥＩｍｋと振動形状導出部２１２から受け取った振動形状を示す情報に基づいて、実施の形態１と同様の計算を行い、式（３９）に示す波腹方位角θｒの推定値を算出する。
　図５は切替判定部２１４の処理アルゴリズムを示した図である。切替判定部２１４は、図５に示すように、振動形状導出部２１２から受け取った時刻ｋでの波腹方位角の観測値と、状態推定部２１３から受け取った時刻ｋでの波腹方位角の推定値を用いて、その差分の絶対値が閾値ｃ以上の場合、切替判定フラグを１に変更する。一方、その差分の絶対値が閾値ｃより小さい場合、切替判定フラグを０にする。すなわち、式（４１）のように切替判定フラグを設定する。
Figure JPOXMLDOC01-appb-M000041
　切替判定部２１４は、このように決定された切替判定フラグを共分散行列調整部２１５と状態推定部２１３に送る。
　図６は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図６に示すように、切替判定フラグが１の場合（ステップＳ３０１：Ｙｅｓ）、システム雑音共分散行列Ｑｋを調整する（ステップＳ３０２）。式（４１）の条件分岐において切替判定フラグが１である場合、波腹方位角θｒの観測値と推定値との差が大きく、現状のフィルタ特性では推定値が観測値に追従していないと判断できる。このため、カルマンフィルタの追従性を高めるために、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋを相対的に大きくする。
　ここで、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくするとき、システム雑音共分散行列Ｑｋの各要素の値を、対応する観測雑音共分散行列Ｒｋの各要素の値に対して相対的に大きくする。行列のどの要素の値を相対的に大きくするかは、推定する対象に応じて変更する。
　共分散行列調整部２１５は、ステップＳ３０２で更新したシステム雑音共分散行列Ｑｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ３０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ３０３）。
　次に、状態推定部２１３の処理アルゴリズムを説明する。図７は状態推定部２１３の処理アルゴリズムを示した図である。図７に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。
　図７に示すように、第２ステップにおいて、切替判定フラグが１である場合は（ステップＳ１１１：Ｙｅｓ）、共分散行列調整部２１５から受け取ったシステム雑音共分散行列Ｑ’を用いて時刻ｋ－１における誤差共分散行列の予測値を、式（４２）を用いて計算する（ステップＳ１１２）。
Figure JPOXMLDOC01-appb-M000042
　さらに、式（４２）で求めた誤差共分散行列の予測値を用いて式（２９），（３３）～（３６）を再計算する（ステップＳ１１３～１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θｒの推定値を計算して（ステップＳ１１８）出力する。
　一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。
　以上説明した本実施の形態２の方位角推定装置２０の動作を図８のフローチャートに沿って説明する。図８は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。
　まず、方位角推定装置２０の演算部２１は初期化を行う（ステップＳ２０１）。具体的には、状態推定部２１３に対し、時刻ｋ＝０における初期値を式（２８）のように設定する。
　次に、方位角推定装置２０の演算部２１のセンサ出力取得部２１１が、円状物体１０に備えられたセンサ１１が出力するセンサ出力値Ｅｍを取得する（ステップＳ２０２）。センサ出力取得部２１１は、センサ出力値Ｅｍを振動形状導出部２１２に出力し、センサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋを算出して状態推定部２１３に出力する。
　次に、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて、振動形状を導出する（ステップＳ２０３）。具体的には、式（１）～（１３）を用いて、振動形状導出部２１２が、振動形状を示す値である１次共振モードの振動振幅Ａの観測値と、１次共振モードの波腹方位角θｒの観測値と、アクチュエータの交流駆動信号に対する一次共振モードの共振位相特性φｒの観測値と、を算出する。式（１８）に示す観測値は、状態推定部２１３に対して出力され、式（４０）に示す観測値は、切替判定部２１４に対して出力される。
　次に、状態推定部２１３がセンサ出力取得部２１１から入力されたセンサ出力値Ｅｍの時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状の情報をもとに、円状物体１０の状態を推定し、波腹方位角θｒの推定値を算出する。具体的には、式（１４）～（３８）を用いて、センサ出力取得部２１１から入力されたセンサ出力の時間変化を示すＥＲｅｋ，ＥＩｍｋと、振動形状導出部２１２から受け取った振動形状を示す振動振幅Ａと共振位相特性φｒの観測値に基づいて、式（３９）に示す波腹方位角θｒの推定値を算出する（ステップＳ２０４）。
　次に、切替判定部２１４が切替の要否の判定を行う。切替判定では、式（４１）をもとに切替判定フラグの値を設定する。切替判定の結果、振動形状導出部２１２が出力する波腹方位角の観測値と状態推定部２１３が出力する波腹方位角の推定値の差の絶対値が閾値より小さくて切替判定フラグが０である場合（ステップＳ２０５：Ｎｏ）、再計算を行わずその時点の状態量の推定値から波腹方位角θｒの推定値を算出して処理を終了する。
　一方、切替判定フラグが１であった場合、共分散行列調整部２１５がシステム雑音共分散行列の変更を行い（ステップＳ２０６）、当該共分散行列を状態推定部２１３に渡す。そして、状態推定部２１３は、共分散行列調整部２１５から受け取ったシステム雑音共分散行列を用いて波腹方位角θｒの推定値を再計算する（ステップＳ２０７）。最後に、演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）ステップＳ２０１に戻って次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）処理を終了する。
　本実施の形態２のアルゴリズムに従って波腹方位角θｒの推定値を算出することで、波腹方位角θｒの観測値と推定値の差の絶対値が大きい場合には追従性を高めたカルマンフィルタにより波腹方位角θｒの推定値を再計算することで、推定値を観測値に追従させることができ、波腹方位角θｒの観測値と推定値の差分の絶対値が小さい場合には、耐ノイズ性を高めたカルマンフィルタにより波腹方位角θｒの推定値を低ノイズ化できる。
　つまり、本実施の形態２の構成は、波腹方位角θｒの推定値を高精度で推定するだけでなく、観測値の変化に応じて、応答性と耐ノイズ性を変化させることができるフィルタを備えていると言える。このようなフィルタは、角速度入力を検出した際の波腹方位角の変化がノイズの大きさに対して十分に大きいデバイスに対して大きな効果を発揮する。例えば、半球共振型ジャイロに対して有効である。角速度が入力されない場合は耐ノイズ性を高めて低ノイズ化し、角速度が入力された場合は入力を正確に検知するために追従性を高めることができる。これにより角速度入力に遅延なく応答しつつ、入力がないときのノイズを極めて小さく抑えることができる。
　以上説明したように、本実施の形態２によれば、ジャイロシステム２の方位角推定装置２０は、振動形状導出部２１２が、センサ出力値Ｅｍに基づいて波腹方位角θｒの観測値を算出して切替判定部２１４に渡し、状態推定部２１３が、センサ出力値Ｅｍの時間変化と振動形状の情報に基づいて波腹方位角θｒの推定値を算出して切替判定部２１４に渡す。切替判定部２１４は、波腹方位角θｒの観測値と推定値の差の絶対値が大きい場合には、切替が必要と判断し、共分散行列調整部２１５がシステム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくして、状態推定部２１３が、当該システム雑音共分散行列Ｑｋを用いて波腹方位角θｒの推定値を再計算する。一方、切替が不要と判断された場合は、再計算前の状態量の推定値から波腹方位角θｒの推定値を算出することとした。これにより、波腹方位角θｒの観測値の変化に応じて、応答性と耐ノイズ性のいずれを優先させるかを切り替えることが可能となる。
実施の形態３．
　実施の形態３に係るジャイロシステム２について、図６－９を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２と同様であるが、状態推定部２１３、切替判定部２１４、共分散行列調整部２１５の動作が実施の形態２と異なる。なお、図６，７，８は実施の形態２と共通の図である。
　実施の形態２では、図８に示す波腹方位角推定処理のフローチャートのステップＳ２０４の段階において、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に小さくすることで耐ノイズ性を高めた。そして、式（４１）に示した切替判定を行い、波腹方位角θｒｋの観測値と推定値の差分の絶対値が閾値ｃ以上の場合に、ステップＳ２０６の段階で、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくして波腹方位角θｒｋの推定値の再計算を行った。
　これに対し、本実施の形態３では、ステップＳ２０４の段階において、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくすることで追従性を高める。そして、波腹方位角θｒｋの観測値と推定値の差分の絶対値が閾値ｃより小さい場合に、ステップＳ２０６の段階で、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に小さくして耐ノイズ性を高め、波腹方位角θｒｋの推定値の再計算を行う。
　本実施の形態３に係る方位角推定装置２０の構成について、振動形状導出部２１２が、式（４０）に示す波腹方位角θｒの観測値を算出し、状態推定部２１３が式（３９）に示す波腹方位角θｒの推定値を算出する点は実施の形態２と同じである。以後、実施の形態２と異なる点を説明する。
　図９は切替判定部２１４の処理アルゴリズムを示した図である。切替判定部２１４は、図９に示すように、振動形状導出部２１２から受け取った時刻ｋでの波腹方位角θｒの観測値と、状態推定部２１３から受け取った時刻ｋでの波腹方位角θｒの推定値を用いて、その差分の絶対値が閾値ｃより小さい場合、切替判定フラグを１に変更する。一方、その差分の絶対値が閾値ｃ以上である場合、切替判定フラグを０にする。すなわち、式（４３）のように切替判定フラグを設定する。
Figure JPOXMLDOC01-appb-M000043
　切替判定部２１４は、このように決定された切替判定フラグを共分散行列調整部２１５と状態推定部２１３に送る。
　次に、共分散行列調整部２１５の動作を、図６を用いて説明する。図６は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図６に示すように、切替判定フラグが１の場合（ステップＳ３０１：Ｙｅｓ）、システム雑音共分散行列Ｑｋを調整する（ステップＳ３０２）。式（４３）の条件分岐において切替判定フラグが１である場合、波腹方位角θｒの観測値と推定値との差が小さく、現状のフィルタ特性で推定値が観測値に十分に追従していると判断できる。このため、カルマンフィルタの耐ノイズ性を高めるために、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋを相対的に小さくし、更新したシステム雑音共分散行列Ｑ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ３０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ３０３）。
　状態推定部２１３の処理アルゴリズムは、実施の形態２と同じである。
　以上説明した本実施の形態３の方位角推定装置２０の動作を図８のフローチャートに沿って説明する。図８は演算部２１が実行する波腹方位角推定処理を示すフローチャートである。
　ステップＳ２０１～ステップ２０４までは、実施の形態２と同様であるので説明を省略する。
　ステップＳ２０５において、切替判定部２１４が切替判定を行う。切替判定では、式（４３）をもとに切替判定フラグの値を設定する。切替判定の結果、振動形状導出部２１２が出力する波腹方位角θｒの観測値と状態推定部２１３が出力する波腹方位角θｒの推定値の差の絶対値が閾値以上であり切替判定フラグが０である場合（ステップＳ２０５：Ｎｏ）、再計算を行わずその時点の状態量の推定値から波腹方位角θｒの推定値を算出して処理を終了する。
　一方、切替判定の結果、振動形状導出部２１２が出力する波腹方位角θｒの観測値と状態推定部２１３が出力する波腹方位角θｒの推定値の差の絶対値が閾値より小さく切替判定フラグが１であった場合、共分散行列調整部２１５がシステム雑音共分散行列の変更を行い（ステップＳ２０６）、当該共分散行列を状態推定部２１３に渡す。そして、状態推定部２１３は、共分散行列調整部２１５から受け取ったシステム雑音共分散行列を用いて波腹方位角θｒの推定値を再計算する（ステップＳ２０７）。最後に、演算部２１は動作の終了判定を行い、終了指示がない場合は（ステップＳ２０８：Ｎｏ）ステップＳ２０１に戻って次の計算サイクルに移り、終了指示があった場合は（ステップＳ２０８：Ｙｅｓ）動作を停止する。
　以上説明したように、本実施の形態３によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算する際に、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に大きくしておく。切替判定部２１４は、波腹方位角θｒの観測値と推定値の差の絶対値が小さい場合には、切替が必要と判断し、システム雑音共分散行列Ｑｋを観測雑音共分散行列Ｒｋに対して相対的に小さくして、波腹方位角θｒの推定値を再計算する。一方、切替が不要と判断された場合は、再計算前の状態量の推定値から波腹方位角θｒの推定値を算出することとした。これにより、実施の形態２と同様に、波腹方位角θｒの観測値の変化に応じて、応答性と耐ノイズ性のいずれを優先させるかを切り替えることが可能となる。
実施の形態４．
　実施の形態４に係るジャイロシステム２について、図１０，１１を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２，３と同様であるが、状態推定部２１３と共分散行列調整部２１５の動作が実施の形態２，３と異なる。
　実施の形態２，３では、共分散行列調整部２１５において、システム雑音共分散行列Ｑｋを変更することでカルマンフィルタの追従性と耐ノイズ性を調整したが、本実施の形態４では、共分散行列調整部２１５が、図１０に示すように、観測雑音共分散行列Ｒｋを変更することで、カルマンフィルタの追従性と耐ノイズ性を調整する。
　共分散行列調整部２１５の動作を、図１０を用いて説明する。図１０は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図１０に示すように、切替判定フラグが１の場合（ステップＳ４０１：Ｙｅｓ）、観測雑音共分散行列Ｒｋを調整する（ステップＳ４０２）。更新した観測雑音共分散行列Ｒｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ４０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ４０３）。
　観測雑音共分散行列Ｒｋの調整により、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に大きくなればカルマンフィルタの追従性が高まり、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に小さくなればカルマンフィルタの耐ノイズ性が高まる。よって、理想とするフィルタの追従性と耐ノイズ性に応じて、システム雑音共分散行列Ｑｋに対する観測雑音共分散行列Ｒｋの相対的な大きさを変更する。
　次に、状態推定部２１３の処理アルゴリズムを、図１１を用いて説明する。図１１は状態推定部２１３の処理アルゴリズムを示した図である。図１１に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。
　本実施の形態４においては、共分散行列調整部２１５が観測雑音共分散行列Ｒｋを調整するため、実施の形態２，３における式（４２）を計算する必要がなくなり、代わりに調整した観測雑音共分散行列Ｒｋ’を用いてカルマンゲインＫｋを再計算する。すなわち、切替判定フラグが１である場合において（ステップＳ１１１：Ｙｅｓ）、時刻ｋにおけるカルマンゲインＫｋを、式（４４）を用いて再計算する（ステップＳ１１３）。
Figure JPOXMLDOC01-appb-M000044
　式（４４）で求めたカルマンゲインＫｋを用いて式（３３）～（３６）を再計算する（ステップＳ１１４～１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θｒの推定値を導出して（ステップＳ１１８）出力する。
　一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。
　以上説明した本実施の形態４に係る方位角推定装置２０の動作は、実施の形態２又は３と同様である。
　以上説明したように、本実施の形態４によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算又は再計算する際に、観測雑音共分散行列Ｒｋを変更し、システム雑音共分散行列Ｑｋに対して相対的に大きく又は小さくすることとした。これにより、実施の形態２及び３と同様に、カルマンフィルタの追従性と耐ノイズ性を調整することができるとともに、誤差共分散行列の予測値の計算を省略できるため、計算処理時間を短縮することができる。
実施の形態５．
　実施の形態５に係るジャイロシステム２について、図１２，１３を用いて説明する。ジャイロシステム２の全体構成及び方位角推定装置２０の構成は実施の形態２－４と同様であるが、状態推定部２１３と共分散行列調整部２１５の動作が実施の形態２－４と異なる。
　実施の形態２－４では、共分散行列調整部２１５において、システム雑音共分散行列Ｑｋ又は観測雑音共分散行列Ｒｋを変更することでカルマンフィルタの追従性と耐ノイズ性を調整したが、本実施の形態４では、共分散行列調整部２１５が、図１２に示すように、システム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋの両方を変更することで、カルマンフィルタの追従性と耐ノイズ性を調整する。
　共分散行列調整部２１５の動作を、図１２を用いて説明する。図１２は共分散行列調整部２１５の処理アルゴリズムを示した図である。共分散行列調整部２１５は、図１２に示すように、切替判定フラグが１の場合（ステップＳ５０１：Ｙｅｓ）、システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋを調整する（ステップＳ５０２）。更新したシステム雑音共分散行列Ｑｋ’及び観測雑音共分散行列Ｒｋ’を状態推定部２１３に渡す。一方、切替判定フラグが０の場合（ステップＳ５０１：Ｎｏ）、共分散行列調整部２１５は処理を行わず計算を終了する（ステップＳ５０３）。
　システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋの調整により、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に大きくなればカルマンフィルタの追従性が高まり、観測雑音共分散行列Ｒｋに対してシステム雑音共分散行列Ｑｋが相対的に小さくなればカルマンフィルタの耐ノイズ性が高まる。よって、理想とするフィルタの追従性と耐ノイズ性に応じて、システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋを変更する。
　次に、状態推定部２１３の処理アルゴリズムを、図１３を用いて説明する。図１３は状態推定部２１３の処理アルゴリズムを示した図である。図１３に示した処理のうち、第１ステップに含まれている処理は、実施の形態１に係る状態推定部２１３の処理と同様であるため、説明は省略する。
　第２ステップにおいて切替判定フラグが１であるとき、時刻ｋ－１における誤差共分散行列の予測値を、調整したシステム雑音共分散行列Ｑｋ’で再計算し（ステップＳ１１２）、カルマンゲインＫｋを、調整した観測雑音共分散行列Ｒｋ’で再計算する（ステップＳ１１３）。その後、式（３３）～（３６）を再計算する（ステップＳ１１４～１１７）。そして、ステップＳ１１４で算出した時刻ｋでの状態量の推定値から波腹方位角θｒの推定値を導出して（ステップＳ１１８）出力する。
　一方、状態推定部２１３が受け取った切替判定フラグが０である場合は（ステップＳ１１１：Ｎｏ）、再計算を行わず第１ステップ内のステップＳ１０５で導出した状態量の推定値から波腹方位角θｒの推定値を導出して出力する。時刻ｋでのすべての計算が終了した後、状態推定部２１３は時刻をｋからｋ＋１に更新して次の計算サイクルに移行する。このとき、ステップＳ１１６，１１７で計算する状態量の推定値、及び、誤差共分散行列の推定値の更新に関しては、切替判定フラグが１の場合は再計算した値で更新を行う。
　以上説明した本実施の形態５に係る方位角推定装置２０の動作は、実施の形態２－４と同様である。
　以上説明したように、本実施の形態５によれば、状態推定部２１３が、波腹方位角θｒの推定値を計算又は再計算する際に、システム雑音共分散行列Ｑｋ及び観測雑音共分散行列Ｒｋを変更し、システム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋとの相対的な大きさを変えることとした。これにより、実施の形態２－４と同様に、カルマンフィルタの追従性と耐ノイズ性を調整することができる。
　このように本発明は、アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得し、センサ出力値に基づいて、円状物体の振動振幅の観測値と、アクチュエータの交流駆動信号に対する円状物体の共振位相特性の観測値と、を導出する。そして、導出した振動振幅の観測値及び共振位相特性の観測値と、センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を推定することとした。これにより、方位角の推定精度を高め、方位角出力の応答性能を良好にすることが可能になる。
　なお、本発明は、本発明の広義の精神と範囲を逸脱することなく、様々な実施の形態及び変形が可能とされるものである。また、上述した実施の形態は、この発明を説明するためのものであり、本発明の範囲を限定するものではない。すなわち、本発明の範囲は、実施の形態ではなく、特許請求の範囲によって示される。そして、特許請求の範囲内及びそれと同等の発明の意義の範囲内で施される様々な変形が、この発明の範囲内とみなされる。
　例えば、上記実施の形態２－５においては、切替判定部２１４が切替判定する基準となる閾値ｃを一つのみ設定していたが、閾値ｃを複数設けて条件を分岐し、それぞれの条件に対してシステム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋの値を設定してもよい。これによれば、複数存在する閾値ｃに応じてシステム雑音共分散行列Ｑｋと観測雑音共分散行列Ｒｋの大きさの比を設定できるため、波腹方位角θｒの観測値に対する推定値の追従性と耐ノイズ性を複数パターン設定することができる。これにより、閾値が一つの場合に生じうる、フィルタ特性を切り替える際の波腹方位角θｒの推定値の急激な変化を抑制し、波腹方位角θｒの観測値に対して推定値を滑らかに追従させることが可能になる。
　また、上記実施の形態１－５においては、円状物体１０の振動を測定するために複数の変位センサを用いるとしたが、速度センサで振動を測定し、得られたデータを積分することで振動の変位を求めてもよい。あるいは、加速度センサで振動を測定し、得られたデータを２階積分することで振動の変位を求めてもよい。これにより、様々な円状物体１０の振動の測定手段を選択できる。
　また、実施の形態１－５では、状態推定部２１３としてカルマンフィルタを利用するとしたが、状態推定部２１３として拡張カルマンフィルタ、アンセンテッドカルマンフィルタ又はアンサンブルカルマンフィルタを含むカルマンフィルタを応用したフィルタを用いてもよい。例えば、アンセンテッドカルマンフィルタを利用した場合、状態量が非線形状態空間モデルを持つ場合にも適用できる。また、アンサンブルカルマンフィルタを利用した場合は、状態量が非線形・非ガウス状態空間モデルを持つ場合にも適用できる。
　あるいは、状態推定部２１３として粒子フィルタを含む状態推定手段を用いてもよい。粒子フィルタを利用した場合、状態量が非線形・非ガウス状態空間モデルを持つ場合にも適用できる。
　また、演算部２１が実行した処理のプログラムを、既存のコンピュータで実行させることにより、当該コンピュータを本発明に係る方位角推定装置として機能させることも可能である。
　このようなプログラムの配布方法は任意であり、例えば、ＣＤ－ＲＯＭ（Compact Disc Read-Only Memory）、ＤＶＤ（Digital Versatile Disc）、ＭＯ（Magneto Optical Disc）、メモリカード、又は他のコンピュータ読み取り可能な記録媒体に格納して配布してもよいし、インターネットや他の通信ネットワークを介して配布してもよい。
　１，２　ジャイロシステム、１０　円状物体、１１　センサ、２０　方位角推定装置、２１　演算部、２２　記憶部、２１１　センサ出力取得部、２１２　振動形状導出部、２１３　状態推定部、２１４　切替判定部、２１５　共分散行列調整部。

Claims (18)

  1. 　アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから、変位を示すセンサ出力値を取得するセンサ出力取得部と、
    　前記センサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する振動形状導出部と、
    　前記振動形状導出部が導出した前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、を用いた観測方程式及び状態方程式に基づいて波腹方位角推定値を計算する状態推定部と、
    　を備えた方位角推定装置。
  2. 　前記観測方程式は、共振する前記円状物体の時間的及び空間的な変形を表し、
    　前記状態推定部は、観測雑音共分散行列とシステム雑音共分散行列を用いた前記観測方程式及び前記状態方程式の計算により、前記波腹方位角推定値を計算する、
    　請求項１に記載の方位角推定装置。
  3. 　前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算する、
    　請求項２に記載の方位角推定装置。
  4. 　前記振動形状導出部は、前記振動振幅の観測値と前記共振位相特性の観測値に加えて、前記センサ出力値に基づいて波腹方位角の観測値を導出し、
    　前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が計算した前記波腹方位角の推定値との差分の絶対値に基づいて、前記観測雑音共分散行列又は前記システム雑音共分散行列の切替を要するか否かを判定する切替判定部と、
    　前記切替判定部の判定結果に基づいて、前記観測雑音共分散行列と前記システム雑音共分散行列の相対的な大きさを調整する共分散行列調整部と、を更に備える、
    　請求項２に記載の方位角推定装置。
  5. 　前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算し、
    　前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に大きく変更し、
    　前記状態推定部が、変更後の前記システム雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
    　請求項４に記載の方位角推定装置。
  6. 　前記状態推定部は、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に大きく設定して前記波腹方位角推定値を計算し、
    　前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記観測雑音共分散行列に対して前記システム雑音共分散行列を相対的に小さく変更し、
    　前記状態推定部が、変更後の前記システム雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
    　請求項４に記載の方位角推定装置。
  7. 　前記状態推定部は、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に大きく設定して前記波腹方位角推定値を計算し、
    　前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に小さく変更し、
    　前記状態推定部が、変更後の前記観測雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
    　請求項４に記載の方位角推定装置。
  8. 　前記状態推定部は、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に小さく設定して前記波腹方位角推定値を計算し、
    　前記共分散行列調整部は、前記切替判定部が切替を要すると判定した場合に、前記システム雑音共分散行列に対して前記観測雑音共分散行列を相対的に大きく変更し、
    　前記状態推定部が、変更後の前記観測雑音共分散行列を用いて前記波腹方位角の推定値を再計算する、
    　請求項４に記載の方位角推定装置。
  9. 　前記切替判定部は、前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が計算した前記波腹方位角の推定値との差分の絶対値が予め定めた閾値以上である場合に、切替を要すると判定する、
    　請求項５又は７に記載の方位角推定装置。
  10. 　前記切替判定部は、前記振動形状導出部が導出した前記波腹方位角の観測値と、前記状態推定部が推定した前記波腹方位角の推定値との差分の絶対値が予め定めた閾値より小さい場合に、切替を要すると判定する、
    　請求項６又は８に記載の方位角推定装置。
  11. 　前記切替判定部は、前記閾値を複数有しており、各閾値に応じて、前記システム雑音共分散行列と前記観測雑音共分散行列の相対的な大きさを変更する、
    　請求項９又は１０に記載の方位角推定装置。
  12. 　前記センサは、変位センサ、速度センサ及び加速度センサの少なくとも１つである、
    　請求項１から１１のいずれか１項に記載の方位角推定装置。
  13. 　前記状態推定部は、カルマンフィルタを含み、
    　前記共分散行列調整部は、前記カルマンフィルタの追従性又は耐ノイズ性の要求に応じて、前記システム雑音共分散行列と前記観測雑音共分散行列の相対的な大きさを変更する、
    　請求項４から１１のいずれか１項に記載の方位角推定装置。
  14. 　前記状態推定部は、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、アンサンブルカルマンフィルタの少なくとも１つを含む、
    　請求項１３に記載の方位角推定装置。
  15. 　前記状態推定部は、粒子フィルタを含む、
    　請求項１から１２のいずれか１項に記載の方位角推定装置。
  16. 　請求項１から１５のいずれか１項に記載の方位角推定装置を備え、
    　前記円状物体は半球形状を有し、
    　前記センサ出力取得部は、前記アクチュエータの前記交流駆動信号により共振モードを励起された前記半球に配置された複数の前記センサから、変位を示す前記センサ出力値を取得する、
    　ジャイロシステム。
  17. 　アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから取得した、変位を示すセンサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する観測値導出ステップと、
    　前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、に基づいて波腹方位角推定値を計算する方位角計算ステップと、
    　を有する方位角推定方法。
  18. 　コンピュータを、
    　アクチュエータの交流駆動信号により共振モードを励起された円状物体に配置された複数のセンサから取得した、変位を示すセンサ出力値に基づいて、前記円状物体の振動振幅の観測値と、前記アクチュエータの前記交流駆動信号に対する前記円状物体の共振位相特性の観測値と、を導出する振動形状導出部、
    　前記振動振幅の観測値及び前記共振位相特性の観測値と、前記センサ出力値の時間変化と、に基づいて波腹方位角推定値を計算する状態推定部、
    　として機能させるプログラム。
PCT/JP2019/022131 2019-06-04 2019-06-04 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム WO2020245902A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
PCT/JP2019/022131 WO2020245902A1 (ja) 2019-06-04 2019-06-04 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム
JP2021524532A JP7066062B2 (ja) 2019-06-04 2019-06-04 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2019/022131 WO2020245902A1 (ja) 2019-06-04 2019-06-04 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム

Publications (1)

Publication Number Publication Date
WO2020245902A1 true WO2020245902A1 (ja) 2020-12-10

Family

ID=73652014

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/022131 WO2020245902A1 (ja) 2019-06-04 2019-06-04 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム

Country Status (2)

Country Link
JP (1) JP7066062B2 (ja)
WO (1) WO2020245902A1 (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114397816A (zh) * 2021-12-15 2022-04-26 合肥工业大学 基于状态反馈x-LMS算法的发动机主动悬置控制方法
CN114396938A (zh) * 2021-12-07 2022-04-26 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种舰船捷联惯导系统的高精度初始对准方法
CN116975541A (zh) * 2023-09-21 2023-10-31 深圳市盘古环保科技有限公司 一种垃圾填埋场存量垃圾自动筛分系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001153680A (ja) * 1999-11-26 2001-06-08 Japan Aviation Electronics Industry Ltd 慣性装置
JP2006234710A (ja) * 2005-02-28 2006-09-07 Seiko Epson Corp ジャイロ信号検出装置
JP2010066254A (ja) * 2008-09-11 2010-03-25 Northrop Grumman Guidance & Electronics Co Inc 自己較正型ジャイロスコープ・システム
JP2019078560A (ja) * 2017-10-20 2019-05-23 シャープ株式会社 ジャイロセンサのオフセット補正装置、オフセット補正プログラム、歩行者自律航法装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2001153680A (ja) * 1999-11-26 2001-06-08 Japan Aviation Electronics Industry Ltd 慣性装置
JP2006234710A (ja) * 2005-02-28 2006-09-07 Seiko Epson Corp ジャイロ信号検出装置
JP2010066254A (ja) * 2008-09-11 2010-03-25 Northrop Grumman Guidance & Electronics Co Inc 自己較正型ジャイロスコープ・システム
JP2019078560A (ja) * 2017-10-20 2019-05-23 シャープ株式会社 ジャイロセンサのオフセット補正装置、オフセット補正プログラム、歩行者自律航法装置

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114396938A (zh) * 2021-12-07 2022-04-26 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种舰船捷联惯导系统的高精度初始对准方法
CN114396938B (zh) * 2021-12-07 2023-08-08 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种舰船捷联惯导系统的高精度初始对准方法
CN114397816A (zh) * 2021-12-15 2022-04-26 合肥工业大学 基于状态反馈x-LMS算法的发动机主动悬置控制方法
CN114397816B (zh) * 2021-12-15 2023-06-27 合肥工业大学 基于状态反馈x-LMS算法的发动机主动悬置控制方法
CN116975541A (zh) * 2023-09-21 2023-10-31 深圳市盘古环保科技有限公司 一种垃圾填埋场存量垃圾自动筛分系统
CN116975541B (zh) * 2023-09-21 2024-01-09 深圳市盘古环保科技有限公司 一种垃圾填埋场存量垃圾自动筛分系统

Also Published As

Publication number Publication date
JPWO2020245902A1 (ja) 2021-10-14
JP7066062B2 (ja) 2022-05-12

Similar Documents

Publication Publication Date Title
WO2020245902A1 (ja) 方位角推定装置、ジャイロシステム、方位角推定方法及びプログラム
JP5421693B2 (ja) 自己較正型ジャイロスコープ・システム
CN105785999B (zh) 无人艇航向运动控制方法
JP4317173B2 (ja) 移動物体の方向検出方法及びそのシステム
JP6191103B2 (ja) 移動状態算出方法及び移動状態算出装置
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
RU2476823C2 (ru) Способ измерения при помощи гироскопической системы
JP2010539478A (ja) 統合された全地球的航法衛星システム・センサおよび慣性センサを有する自動ブレード制御システム
WO2017150106A1 (ja) 車載装置、及び、推定方法
JP2014228495A (ja) 自己位置推定装置及び方法
Yousuf et al. Sensor fusion of INS, odometer and GPS for robot localization
US8342024B2 (en) Gyroscopic measurement by a vibratory gyroscope
EP3189303B1 (en) Pedestrian navigation devices and methods
JP3726884B2 (ja) 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
CN114509057A (zh) 一种谐振陀螺仪全角模式控制方法
EP3108208A1 (en) Inertial device, control method and program
JP5052165B2 (ja) 船舶用自動操舵装置
JP2013253985A (ja) 衛星信号から導かれるドリフト推定を使用したｇｐｓ受信機の調整可能クロック
Parikh et al. Comparison of attitude estimation algorithms with IMU under external acceleration
Huang et al. Development of attitude and heading reference systems
JP2006038650A (ja) 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム
RU2754396C1 (ru) Адаптивный способ коррекции углов ориентации БИНС
JP5145371B2 (ja) 慣性航法装置
JP5569276B2 (ja) 車両用現在位置検出装置
JP3764845B2 (ja) データ処理装置および記録媒体

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19931940

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2021524532

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19931940

Country of ref document: EP

Kind code of ref document: A1