KR101922700B1 - 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치 - Google Patents

가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치 Download PDF

Info

Publication number
KR101922700B1
KR101922700B1 KR1020170071721A KR20170071721A KR101922700B1 KR 101922700 B1 KR101922700 B1 KR 101922700B1 KR 1020170071721 A KR1020170071721 A KR 1020170071721A KR 20170071721 A KR20170071721 A KR 20170071721A KR 101922700 B1 KR101922700 B1 KR 101922700B1
Authority
KR
South Korea
Prior art keywords
quaternion
matrix
quot
sensor
angular velocity
Prior art date
Application number
KR1020170071721A
Other languages
English (en)
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 KR1020170071721A priority Critical patent/KR101922700B1/ko
Priority to US15/984,923 priority patent/US10788324B2/en
Priority to CN201810585981.9A priority patent/CN109030867B/zh
Application granted granted Critical
Publication of KR101922700B1 publication Critical patent/KR101922700B1/ko

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01V—GEOPHYSICS; GRAVITATIONAL MEASUREMENTS; DETECTING MASSES OR OBJECTS; TAGS
    • G01V1/00—Seismology; Seismic or acoustic prospecting or detecting
    • G01V1/38—Seismology; Seismic or acoustic prospecting or detecting specially adapted for water-covered areas
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
    • G01P3/42—Devices characterised by the use of electric or magnetic means
    • G01P3/44—Devices characterised by the use of electric or magnetic means for measuring angular speed
    • 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/5776—Signal processing not specific to any of the devices covered by groups G01C19/5607 - G01C19/5719
    • 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
    • G01P15/14—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of gyroscopes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01V—GEOPHYSICS; GRAVITATIONAL MEASUREMENTS; DETECTING MASSES OR OBJECTS; TAGS
    • G01V3/00—Electric or magnetic prospecting or detecting; Measuring magnetic field characteristics of the earth, e.g. declination, deviation
    • G01V3/40—Electric or magnetic prospecting or detecting; Measuring magnetic field characteristics of the earth, e.g. declination, deviation specially adapted for measuring magnetic field characteristics of the earth
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/401—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by control arrangements for measuring, e.g. calibration and initialisation, measuring workpiece for machining purposes
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/408—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by data handling or data format, e.g. reading, buffering or conversion of data
    • G05B19/4086—Coordinate conversions; Other special calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/37—Measurements
    • G05B2219/37024—Measure single value, parameter with two detectors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/37—Measurements
    • G05B2219/37124—Magnetic sensor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/37—Measurements
    • G05B2219/37388—Acceleration or deceleration, inertial measurement

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Remote Sensing (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Human Computer Interaction (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Geophysics (AREA)
  • Environmental & Geological Engineering (AREA)
  • Geology (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Signal Processing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Oceanography (AREA)
  • Acoustics & Sound (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

본 발명은 물리적인 자이로 센서를 이용하지 않고, 가속도 센서와 지자기 센서의 측정값을 활용하여 각속도를 산출할 수 있도록 하는 각속도 산출 방법 및 장치에 관한 것으로, 에러 쿼터니온을 기반으로 상태 변수와 상태 변수의 분산을 도출하고, 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계; 상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도 센서 출력과 지자기 센서의 출력을 재정의한 후, 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계; 이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시킨 후, 상기 전이된 상태 변수의 분산과 상기 칼만 필터의 관측 행렬을 기반으로 상기 칼만 필터의 이득을 산출하는 단계; 상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상한 후, 상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 단계; 및 상기 쿼터니온을 기반으로 각속도를 산출하는 단계;를 포함할 수 있다.

Description

가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치{Method and Apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor}
본 발명은 각속도 산출 방법 및 장치에 관한 것으로, 특히 가속도 센서와 지자기 센서의 측정값을 이용하여 각속도를 산출할 수 있도록 하는 각속도 산출 방법 및 장치에 관한 것이다.
각속도 정보는 운동 중인 대상물의 운동 정보가 필요한 여러 분야에서 활용되는 중요한 정보이다. 각속도(또는 회전 각속도)는 자동차 산업의 경로 안내(car navigation)에 필요한 진행 방향 및 회전 방향 계산에 이용되고, 해양 선박 분야에서 같은 목적으로 활용된다. 최근에 각광을 받고 있는 드론이나 자율 주행 차량과 같은 분야에서도 진행 방향 및 자세 제어를 위해 사용되고 있어, 그 필요성이 더욱 증가되고 있다. 또한 게임기의 컨트롤러, 증강 현실 분야에서의 사용자의 방향 입력 장치 등에서도 활용되고 있으며, 나아가 휴대폰을 입력장치로 사용하는 게임 나침반 등의 분야에서의 각도와 위치 제어 및 동작 인식에 활용된다. 이와 같이 각속도 정보를 필요로 하는 추세는 계속 확대되고 있다.
각속도는 공간상의 물체가 이동을 할 때 회전 각 정보와 위치 정보를 측정할 때 사용된다. 이를 위해서 회전 가속도를 획득할 수 있는 물리적인 자이로 센서를 사용되고 있다.
자이로 센서는 다양한 원리를 이용하여 각속도 정보를 계산할 수 있으나, 주로 코리올리(Coriolis)힘을 이용한다. 물체가 회전하면서 진행할 때 진행 방향의 90° 방향으로 코리올리 힘이 발생하며, 이 힘은 회전에 의해서만 발생한다. 일반적으로 각속도를 측정할 때 직접 측정할 수 있는 방법이 용이하지 않아서 먼저 코리올리 힘을 측정하고 변환시켜 각속도를 측정한다.
다만, 이러한 측정 원리를 구현하기 위해서는 센서 내에 감지 구조물을 회전 또는 진동을 발생시켜야 하는데, 이는 센서 소형화에 장애 요소로 작용한다. 이런 특성은 최근 널리 확대되고 있는 MEMS(Micro-Electro Mechanical System)기술을 이용한 MEMS 자이로 센서에도 동일하게 문제를 야기시킨다. 대부분 MEMS 자이로 센서는 소리굽쇠(tuning fork)방식, 진동-휠(vibration-wheel)방식, 푸코 추(Foucault pendulum)방식과 와인잔 튜닝(wine glass tuning)방식을 이용하고 있는데, 이들 역시 센서 내부에 정지 상태가 아닌 움직임이 있는 감지 구조물을 포함해야 하는 구조이다.
또한 센서내에 작은 공간내에 운동 상태를 유지하는 구조물을 필요로 하기 때문에, 이로 인한 제약도 발생한다. 즉, 설계 제작상의 난이도를 증가시키며 제조비용을 증가시킨다. 감지 구조물을 운동 상태로 유지시키기 위해서 전기적으로 가진을 시켜야 하므로 전류를 소모하여 상대적으로 다른 센서에 비해 소모 전력량이 크다. 이때 소모되는 전류로 말미암아 센서의 내부 온도를 증가시켜 전기적인 안정성을 훼손시킨다. 아울러 내부 구조물이 복잡하여, 기계적인 충격에 대해서도 취약한 특성이 있다.
MEMS 자이로 센서가 소형화, 저 전력화되면서 널리 활용되고 있는 추세에 있으나, 이와같은 문제점들은 MEMS 자이로 센서가 널리 활용되는 데에 장애가 되고 있다.
자이로 센서는 특성상 각속도를 출력하지만 실제로 여러 응용처는 각속도 보다는 각도를 이용하는 경우가 더욱 빈번하다. 각속도에서 각도 정보로 변환하기 위해서는 적분 과정을 거쳐야 한다. 이는 수학적으로 불안정성을 발생시켜 수학적인 포화(saturation)를 발생시키고, 이에 따른 오동작을 유발할 수 있다. 모든 센서들은 불가피하게 오프셋을 포함하는데, 자이로 센서도 예외가 아니며, 제품 사양에 명시한 오프셋을 허용하고 있다. 비록 오프셋이 작다 하더라도 충분한 시간이 지나면서 주기적으로 그 값을 리셋하지 않으면, 포화현상을 피할 수 없게 된다. 이런 특성 역시 제용 상에 제한을 주는 특성이다.
국내공개특허 제10-2016-0133259(공개일자: 2016년11월22일)
이에 상기와 같은 문제점을 해결하기 위한 것으로서, 본 발명은 물리적인 자이로 센서를 구비하지 않고, 가속도 센서와 지자기 센서의 측정값을 이용하여 각속도를 산출할 수 있도록 하는 새로운 방식의 각속도 산출 방법 및 장치를 제공하고자 한다.
본 발명의 목적은 이상에서 언급한 목적으로 제한되지 않으며, 언급되지 않은 또 다른 목적들은 아래의 기재로부터 본 발명이 속하는 통상의 지식을 가진 자에게 명확하게 이해될 수 있을 것이다.
상기 과제를 해결하기 위한 수단으로서, 본 발명의 일 실시 형태에 따르면, 에러 쿼터니온을 기반으로 상태 변수와 상태 변수의 분산을 도출하고, 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계; 상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도 센서 출력과 지자기 센서의 출력을 재정의한 후, 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계; 이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시킨 후, 상기 전이된 상태 변수의 분산과 상기 칼만 필터의 관측 행렬을 기반으로 상기 칼만 필터의 이득을 산출하는 단계; 상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상한 후, 상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 단계; 및 상기 쿼터니온을 기반으로 각속도를 산출하는 단계; 포함하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법을 제공한다.
상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는 기준 좌표계와 센서 좌표계간의 상대 각도 변화를 표현하는 쿼터니온(q=[q0 qx qy qz]')의 에러 쿼터니온([qex qey qez]')을 상태 변수(x)로 설정하는 것을 특징으로 한다.
또한 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는 현재 상태(state)와 이전의 상태(state)가 시간 지연(τ)만 발생하는 통계적 상관관계인 것이라는 가정하에 상기 상태 변수(x)를 "
Figure 112017054692895-pat00001
"의 식으로 표현하며, 상기 F는 "
Figure 112017054692895-pat00002
"으로 표현되는 전이 행렬이며, 상기 τ는 상태들 간에 연관되는 정도를 나타내는 시간 인자인 것으로 특징으로 한다.
그리고 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는 상기 상태 변수의 분산을 "
Figure 112017054692895-pat00003
"의 식에 따라 산출하며, 상기 x는 상태 변수이고, 상기
Figure 112017054692895-pat00004
는 추정된 상태 변수, 상기 '는 세로 행렬(transpose matrix)인 것을 특징으로 한다.
그리고 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는 "
Figure 112017054692895-pat00005
"의 식을 기반으로 상기 쿼터니온 기반의 회전 행렬을 상기 에러 쿼터니온 기반의 회전 행렬로 변환하며, 상기 쿼터니온 기반의 회전 행렬은 "
Figure 112017054692895-pat00006
"이고, 상기 에러 쿼터니온 기반의 회전 행렬은 "
Figure 112017054692895-pat00007
"이며, 상기 qex , qey , qez는 에러 쿼터니온의 벡터 성분 인자이고, 상기
Figure 112017054692895-pat00008
는
Figure 112017054692895-pat00009
를 의미하고, 상기 q0은 쿼터니온에서
Figure 112017054692895-pat00010
스칼라(scalar)에 해당하는 인자인 것을 특징으로 한다.
이때, 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는 상기 qex, 상기 qey, 상기 qez이 미소량이라는 가정하에, "
Figure 112017054692895-pat00011
"으로 표현되는 상기 에러 쿼터니온 기반의 회전 행렬을 "
Figure 112017054692895-pat00012
"으로 간략화하는 것을 특징으로 한다.
상기 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계는 "
Figure 112017054692895-pat00013
"으로 표현되는 상기 에러 쿼터니온(qe)과 쿼터니온 기반의 회전 행렬(C(q))간 상관관계를 기반으로 가속도 센서 출력(Ya)및 지자기 센서의 출력(Ym)을 재정의하며, 상기 가속도 센서 출력(Ya)은 "
Figure 112017054692895-pat00014
"이고, 상기 지자기 센서의 출력(Ym)은 "
Figure 112017054692895-pat00015
"이며, 상기
Figure 112017054692895-pat00016
은 추정된 쿼터니온 기반의 회전 행렬이고, 상기 I3×3은 3×3 단위 행렬, 상기
Figure 112017054692895-pat00017
는 중력 가속도 벡터이고, 상기 Ba는 선형 가속도이고, 상기 Va는 가속도 센서 센서의 잡음이고, 상기
Figure 112017054692895-pat00018
는 지구 지자기 벡터이고, 상기 Vm은 지자기 센서의 잡음인 것을 특징으로 한다.
그리고 상기 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계는 상기 가속도 센서 출력(Ya)과 상기 지자기 센서의 출력(Ym)을 기반으로 "
Figure 112017054692895-pat00019
"의 칼만 필터의 관측 행렬을 산출하고, "
Figure 112017054692895-pat00020
"의 상기 칼만 필터의 출력 행렬을 산출하는 것을 특징으로 한다.
상기 칼만 필터의 이득을 산출하는 단계는 "
Figure 112017054692895-pat00021
"의 식에 따라 상기 상태 변수(x)를 전이하고,
Figure 112017054692895-pat00022
"의 식에 따라 상기 상태 변수의 분산(p)을 전이하며, 상기 Fk는 "
Figure 112017054692895-pat00023
"의 식으로 표현되는 이산화된 전이 행렬이고, 상기 Qdk는 "
Figure 112017054692895-pat00024
"의 식으로 표현되는 Q의 이산화된 프로세스 잡음 분산이고, 상기 T는 샘플링 시간인 것을 특징으로 한다.
또한 상기 칼만 필터의 이득을 산출하는 단계는 "
Figure 112017054692895-pat00025
"의 식으로 표현되는 이산화된 전이 행렬을 1차 선형식으로 가정하여 "
Figure 112017054692895-pat00026
"으로 간략화하며, 상기 F는 "
Figure 112017054692895-pat00027
"로 표현되는 전이 행렬이고, 상기 τ는 상태들 간에 연관되는 정도를 나타내는 시간 인자인 것을 특징으로 한다.
상기 칼만 필터의 이득을 산출하는 단계는 "
Figure 112017054692895-pat00028
"의 식에 따라 상기 칼만 필터의 이득을 산출하며, 상기 R은 측정 센서의 분산으로 "
Figure 112017054692895-pat00029
"의 식으로 표현되며, 상기 I3×3은 3×3 단위 행렬이고, 상기 03×3은 3×3 0 행렬이고, 상기 Ra는 가속도 센서의 분산이고, 상기 Rm는 지자기 센서의 분산인 것을 특징으로 한다.
상기 쿼터니온을 산출하는 단계는 "
Figure 112017054692895-pat00030
"의 식에 따라 상기 상태 변수를 보상하고, "
Figure 112017054692895-pat00031
"의 식에 따라 상기 상태 변수의 분산을 보상하며, 상기 Kk는 칼만 필터의 이득, 상기 Zk는 칼만 필터의 출력 행렬, 상기 Hk는 칼만 필터의 관측 행렬, 상기 I은 3×3 단위 행렬, 상기 R은 측정 센서 에러의 분산인 것을 특징으로 한다.
상기 쿼터니온을 산출하는 단계는 상기 보상된 상태 변수를 에러 쿼터니온으로 설정한 후, "
Figure 112017054692895-pat00032
"의 식에 따라 쿼터니온을 산출하며, 상기
Figure 112017054692895-pat00033
는 추정된 쿼터니온, 상기 qex, qey, qez는 에러 쿼터니온의 3인자 변수인 것을 특징으로 한다.
상기 쿼터니온을 기반으로 각속도를 산출하는 단계는 상기 쿼터니온을 미분하여 "
Figure 112017054692895-pat00034
"의 식으로 표현되는 미분식을 획득한 후, 상기 미분식을 인자별로 정리한 후 이산화하여 "
Figure 112017054692895-pat00035
"의 식으로 표현되는 각속도 산출식을 획득하며, 상기 W(t)는 "
Figure 112017054692895-pat00036
"의 식으로 표현되는 각속도 벡터, 상기 q0,qx,qy,qz는 상기 쿼터니온을 구성하는 4개의 인자, 상기 T는 샘플링 시간인 것을 특징으로 한다.
상기 쿼터니온을 기반으로 각속도를 산출하는 단계는 상기 미분식을 인자별로 정리하여 "
Figure 112017054692895-pat00037
", "
Figure 112017054692895-pat00038
", "
Figure 112017054692895-pat00039
", "
Figure 112017054692895-pat00040
"의 식을 도출 및 정리하여 "
Figure 112017054692895-pat00041
"의 식으로 표현되는 각속도 벡터 산출식을 생성한 후, 상기 각속도 벡터 산출식을 이산화하여 상기 각속도 산출식을 획득하며, 상기 q-1는 "
Figure 112017054692895-pat00042
"의 식으로 표현되는 것을 특징으로 한다.
상기 방법은 상기 쿼터니온에서 오일러 각도를 z축 y축 x축 순서로 회전시켜 산출하는 단계를 더 포함하며, 상기 오일러 각도는 "
Figure 112017054692895-pat00043
", "
Figure 112017054692895-pat00044
",
Figure 112017054692895-pat00045
중 적어도 하나의 식을 기반으로 산출되며, 상기 q0,qx,qy,qz는 상기 쿼터니온을 구성하는 4개의 인자인 것을 특징으로 한다.
그리고 상기 방법은 상기 쿼터니온을 기반으로 오일러 회전 축과 각도를 산출하는 단계를 더 포함하며, 상기 오일러 회전 축은 "
Figure 112017054692895-pat00046
"의 식에 따라 산출되며, 상기 오일러 회전 축 각도는 "
Figure 112017054692895-pat00047
"의 식에 따라 산출되며, 상기
Figure 112017054692895-pat00048
는 "
Figure 112017054692895-pat00049
"으로 표현되는 쿼터니온의 벡터, 상기 i,j,k는 각 축의 단위 벡터, 상기
Figure 112017054692895-pat00050
는 상기
Figure 112017054692895-pat00051
의 절대값인 것을 특징으로 한다.
또한 상기 방법은 상기 쿼터니온을 기반으로 운동 중의 대상체의 선형 가속도를 산출하는 단계를 더 포함하며, 상기 선형 가속도는 "
Figure 112017054692895-pat00052
"의 식에 따라 산출되며, 상기 Ya는 가속도 센서 출력이며, 상기 C(q)는 쿼터니온, 상기
Figure 112017054692895-pat00053
는 중력 가속도 벡터인 것을 특징으로 한다.
상기 과제를 해결하기 위한 수단으로서, 본 발명의 다른 실시 형태에 따르면, 에러 쿼터니온을 설정하는 에러 쿼터니온 설정부; 상기 에러 쿼터니온을 기반으로 상태 변수, 상태 변수의 분산을 도출하고, 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 예측부; 상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도 센서 출력과 지자기 센서의 출력을 재정의하여 칼만 필터의 관측 행렬 및 출력 행렬을 산출하고, 이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시켜 상기 칼만 필터의 이득을 산출하는 필터 구성부; 상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상하는 보상부; 상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 쿼터니온 산출부; 및 상기 쿼터니온을 기반으로 각속도를 산출하는 각속도 획득부를 포함하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치를 제공한다.
상기 예측부는 기준 좌표계와 센서 좌표계간의 상대 각도 변화를 표현하는 쿼터니온(q=[q0 qx qy qz]')의 에러 쿼터니온([qex qey qez]')을 상태 변수(x)로 설정하고, 현재 상태(state)와 이전의 상태(state)가 시간 지연(τ)만 발생하는 통계적 상관관계인 것이라는 가정하에 상기 상태 변수(x)를 "
Figure 112017054692895-pat00054
"의 식으로 표현하며, 상기 F는 "
Figure 112017054692895-pat00055
"으로 표현되는 전이 행렬이며, 상기 τ는 상태들 간에 연관되는 정도를 나타내는 시간 인자인 것으로 특징으로 한다.
또한 상기 예측부는 "
Figure 112017054692895-pat00056
"의 식을 기반으로 상기 쿼터니온 기반의 회전 행렬을 상기 에러 쿼터니온 기반의 회전 행렬로 변환하며, 상기 쿼터니온 기반의 회전 행렬은 "
Figure 112017054692895-pat00057
"이고, 상기 에러 쿼터니온 기반의 회전 행렬은 "
Figure 112017054692895-pat00058
"이며, 상기 qex , qey , qez는 에러 쿼터니온의 벡터 성분 인자이고, 상기
Figure 112017054692895-pat00059
는
Figure 112017054692895-pat00060
를 의미하고, 상기 q0은 쿼터니온에서
Figure 112017054692895-pat00061
스칼라(scalar)에 해당하는 인자인 것을 특징으로 한다.
상기 필터 구성부는 "
Figure 112017054692895-pat00062
"으로 표현되는 상기 에러 쿼터니온(qe)과 쿼터니온 기반의 회전 행렬(C(q))간 상관관계를 기반으로 가속도 센서 출력(Ya)및 지자기 센서의 출력(Ym)을 재정의하며, 상기 재정의 된 가속도 센서 출력(Ya)은 "
Figure 112017054692895-pat00063
"이고, 상기 재정의 된 지자기 센서 출력(Ym)은 "
Figure 112017054692895-pat00064
"이며, 상기
Figure 112017054692895-pat00065
은 추정된 쿼터니온 기반의 회전 행렬이고, 상기 I3×3은 3×3 단위 행렬, 상기
Figure 112017054692895-pat00066
는 중력 가속도 벡터이고, 상기 Ba는 선형 가속도이고, 상기 Va는 가속도 센서 센서의 잡음이고, 상기
Figure 112017054692895-pat00067
는 지구 지자기 벡터이고, 상기 Vm은 지자기 센서의 잡음인 것을 특징으로 한다.
그리고 상기 필터 구성부는 상기 가속도 센서 출력(Ya)과 상기 지자기 센서의 출력(Ym)을 기반으로 "
Figure 112017054692895-pat00068
"의 칼만 필터의 관측 행렬을 산출하고, "
Figure 112017054692895-pat00069
"의 상기 칼만 필터의 출력 행렬을 산출하는 것을 특징으로 한다.
또한 상기 필터 구성부는 "
Figure 112017054692895-pat00070
"의 식에 따라 상기 상태 변수(x)를 전이하고,
Figure 112017054692895-pat00071
"의 식에 따라 상기 상태 변수의 분산(p)을 전이한 후, "
Figure 112017054692895-pat00072
"의 식에 따라 상기 칼만 필터의 이득을 산출하며, 상기 Fk는 "
Figure 112017054692895-pat00073
"의 식으로 표현되는 이산화된 전이 행렬이고, 상기 Qdk는 "
Figure 112017054692895-pat00074
"의 식으로 표현되는 Q의 이산화된 프로세스 잡음 분산이고, 상기 T는 샘플링 시간이고, 상기 Zk는 칼만 필터의 출력 행렬이고, 상기 Hk는 칼만 필터의 관측 행렬이고, 상기 R은 측정 센서 에러의 분산인 것을 특징으로 한다.
상기 보상부는 "
Figure 112017054692895-pat00075
"의 식에 따라 상기 상태 변수를 보상하고, "
Figure 112017054692895-pat00076
"의 식에 따라 상기 상태 변수의 분산을 보상하며, 상기 Kk는 칼만 필터의 이득, 상기 Zk는 칼만 필터의 출력 행렬, 상기 Hk는 칼만 필터의 관측 행렬, 상기 I은 3×3 단위 행렬, 상기 R은 측정 센서 에러의 분산인 것을 특징으로 한다.
상기 쿼터니온 산출부는 상기 보상된 상태 변수를 에러 쿼터니온으로 설정한 후, "
Figure 112017054692895-pat00077
"의 식에 따라 쿼터니온을 산출하며, 상기
Figure 112017054692895-pat00078
는 추정된 쿼터니온, 상기 qex, qey, qez는 에러 쿼터니온의 3인자 변수인 것을 특징으로 한다.
상기 각속도 획득부는 상기 쿼터니온을 미분하여 "
Figure 112018028556177-pat00079
"의 식으로 표현되는 미분식을 획득한 후, 상기 미분식을 인자별로 정리한 후 이산화하여 "
Figure 112018028556177-pat00080
"의 식으로 표현되는 각속도 산출식을 획득하며, 상기 W(t)는 "
Figure 112018028556177-pat00081
"의 식으로 표현되는 각속도 벡터, 상기 q0,qx,qy,qz는 상기 쿼터니온을 구성하는 4개의 인자, 상기 T는 샘플링 시간인 것을 특징으로 한다.
본 발명의 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치는 자이로 센서 대신에 가속도 센서와 지자기 센서의 측정값을 이용하여 각속도를 산출할 수 있도록 한다.
이에 본 발명은 물리적인 자이로 센서를 구비할 필요가 없어, 자이로 센서로 말미암아 발생할 수 있는 각종 문제점 발생 가능성을 사전에 차단하도록 한다. 즉, 장치 구현 비용과 시간이 감소되고, 전력 소모량도 감소될 수 있도록 한다.
도 1은 본 발명의 일 실시예에 따른 각속도 산출 장치의 개략도이다.
도 2는 본 발명의 일 실시예에 따른 각속도 산출부의 상세 구성도이다.
도 3은 본 발명의 일 실시예에 따른 가속도 센서와 지자기 센서 기반의 각속도 산출 방법을 동작 흐름도이다.
도 4는 북반구에서의 지자기 센서와 좌표계를 도시한 도면이다.
도 5는 지자기 센서와 가속도 센서가 동시에 작용하는 좌표계를 도시한 도면이다.
도 6은 기준 좌표계와 사용자 좌표계의 상대 각도 변화를 표현하는 도면이다.
도 7은 본 발명의 다른 실시예에 따른 가속도 센서와 지자기 센서 기반의 각속도 산출 방법을 동작 흐름도이다.
도 8 내지 도 10은 본 발명의 가속도 센서와 지자기 센서 기반의 각속도 산출 방법의 성능을 설명하기 위한 도면이다.
본 발명에서 사용되는 기술적 용어는 단지 특정한 실시 예를 설명하기 위해 사용된 것으로, 본 발명을 한정하려는 의도가 아님을 유의해야 한다. 또한, 본 발명에서 사용되는 기술적 용어는 본 발명에서 특별히 다른 의미로 정의되지 않는 한, 본 발명이 속하는 기술 분야에서 통상의 지식을 가진 자에 의해 일반적으로 이해되는 의미로 해석되어야 하며, 과도하게 포괄적인 의미로 해석되거나, 과도하게 축소된 의미로 해석되지 않아야 한다. 또한, 본 발명에서 사용되는 기술적인 용어가 본 발명의 사상을 정확하게 표현하지 못하는 잘못된 기술적 용어일 때에는, 당업자가 올바르게 이해할 수 있는 기술적 용어로 대체되어 이해되어야 할 것이다. 또한, 본 발명에서 사용되는 일반적인 용어는 사전에 정의되어 있는 바에 따라, 또는 전후 문맥상에 따라 해석되어야 하며, 과도하게 축소된 의미로 해석되지 않아야 한다.
또한, 본 발명에서 사용되는 단수의 표현은 문맥상 명백하게 다르게 뜻하지 않는 한 복수의 표현을 포함한다. 본 발명에서, "구성된다" 또는 "포함한다" 등의 용어는 발명에 기재된 여러 구성 요소들, 또는 여러 단계를 반드시 모두 포함하는 것으로 해석되지 않아야 하며, 그 중 일부 구성 요소들 또는 일부 단계들은 포함되지 않을 수도 있고, 또는 추가적인 구성 요소 또는 단계들을 더 포함할 수 있는 것으로 해석되어야 한다.
이하, 첨부된 도면을 참조하여 본 발명에 따른 바람직한 실시 예를 상세히 설명하되, 도면 부호에 관계없이 동일하거나 유사한 구성 요소는 동일한 참조 번호를 부여하고 이에 대한 중복된 설명은 생략하기로 한다.
또한, 본 발명을 설명함에 있어서 관련된 공지 기술에 대한 구체적인 설명이 본 발명의 요지를 흐릴 수 있다고 판단되는 경우 그 상세한 설명을 생략한다. 또한, 첨부된 도면은 본 발명의 사상을 쉽게 이해할 수 있도록 하기 위한 것일 뿐, 첨부된 도면에 의해 본 발명의 사상이 제한되는 것으로 해석되어서는 아니 됨을 유의해야 한다.
도 1은 본 발명의 일 실시예에 따른 각속도 산출 장치의 개략도이고, 도 2는 본 발명의 일 실시예에 따른 각속도 산출부의 상세 구성도이다.
도 1 및 도 2를 참고하면, 본 발명의 각속도 산출 장치(100)는 가속도 센서(110), 지자기 센서(120), 및 각속도 산출부(130)등을 포함할 수 있다.
가속도 센서(110) 및 지자기 센서(120)는 중력 가속도 정보와 지자기 정보를 측정 및 제공하는 센서이다.
다만, 본 발명의 가속도 센서(110) 및 지자기 센서(120)는 보상(calibration)과정을 거친 후에 각속도 산출에 필요한 정보를 제공하도록 한다. 만약, 가속도 센서(110)에 중력 가속도 이외에 선형 가속도 성분이 포함되어 있거나, 지자기 센서(120)에 오프셋이 포함되어 있으면, 각속도 산출 동작의 정확성이 현저히 저하되기 때문이다.
각속도 산출부(130)는 칼만 필터를 이용하여 가속도 센서(110) 및 지자기 센서(120)의 측정에 이용된 사용자 좌표계와 기준 좌표계간 상대 각도 변화량을 측정하고, 이로부터 각속도(ω)를 산출할 수 있도록 한다. 특히, 본 발명에서는 에러 쿼터니온을 상태 변수로 설정 및 이용하는 방식을 새로이 제안하고, 이를 기반으로 선형화에 유리한 각속도 산출 동작을 수행할 수 있도록 한다.
보다 구체적으로, 본 발명의 각속도 산출부(130)는 에러 쿼터니온을 설정하는 에러 쿼터니온 설정부(131), 상기 에러 쿼터니온을 기반으로 상태 변수, 상태 변수의 분산을 도출하고, 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 예측부(132), 상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도 센서 출력과 지자기 센서의 출력을 재정의하여 칼만 필터의 관측 행렬 및 출력 행렬을 산출하고, 이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시켜 상기 칼만 필터의 이득을 산출하는 필터 구성부(133), 상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상하는 보상부(134), 상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 쿼터니온 산출부(135), 상기 쿼터니온을 기반으로 각속도를 산출하는 각속도 획득부(136)를 포함할 수 있다.
또한, 필요한 경우, 본 발명의 각속도 산출부(130)는 쿼터니온을 기반으로 오일러 각도, 오일러 회전 축과 각도, 선형 가속도, 및 회전 행렬 등과 같은 부가 정보를 추가 산출하는 부가 정보 획득부(137)를 더 포함할 수도 있다.
이와 같이, 본 발명의 각속도 산출 장치(100)는 실제 자이로 센서를 사용하지 않고 가속도 센서(110) 및 지자기 센서(120)를 이용하여 실제 자이로 센서가 발생시키는 각속도를 산출함으로써, 자이로 센서로 말미암아 발생하는 각종 문제점을 모두 회피할 수 있도록 한다.
더하여, 본 발명의 가속도 센서(110) 및 지자기 센서(120)는 각속도 산출 장치(100)를 위해 별도로 구현되는 센서일 수 있으나, 경우에 따라서는 타 장치(예를 들어, 휴대 단말, 차량, 드론, 게임기 등)에 이미 구비되어 있는 속도 센서 및 가속도 센서일 수도 있다.
즉, 본 발명의 각속도 산출 장치는 가속도 센서(110), 지자기 센서(120), 각속도 산출부(130)를 구비하는 하나의 독립된 하드웨어 장치로써 구현될 수 있으나, 경우에 따라서는 각속도 산출부(130)만이 타 장치에 내장되는 임베디드 형태로써 구현되거나, 또는 각속도 산출부(130)만이 타 장치에 내장되거나 타 장치에 다운로드 및 설치되는 소프트웨어 형태로 구현될 수도 있도록 한다.
도 3은 본 발명의 일 실시예에 따른 가속도 센서와 지자기 센서 기반의 각속도 산출 방법을 동작 흐름도이다.
도 3을 참고하면, 본 발명의 각속도 산출 방법은 에러 쿼터니온을 기반으로 상태 변수와 상태 변수의 분산을 도출하는 단계(S1), 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계(S2), 상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도계 출력과 지자기 센서의 출력을 재정의하는 단계(S3), 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계(S4), 이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시키는 단계(S5), 상기 전이된 상태 변수의 분산과 상기 칼만 필터의 관측 행렬을 기반으로 상기 칼만 필터의 이득을 산출하는 단계(S6), 상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상하는 단계(S7), 상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 단계(S8), 상기 쿼터니온을 기반으로 각속도를 산출하는 단계(S9), 종료 요청 여부를 문의하는 단계(S10), 및 종료가 요청되지 않으면 다음 시점의 각속도를 추가 산출하기 위해 단계 S1로 재신입하는 단계(S11)등을 포함할 수 있다.
이하, 도 3 내지 도 6을 참고하여 본 발명의 각속도 산출 방법에 대해 보다 상세히 살펴보기로 한다.
지자기 센서(120)는 어떤 상황에서도 자북(magnetic north)방향을 지시하는 센서이므로, 본 발명에서 절대 방향 센서로 사용된다.
도 4는 북반구에서의 지자기 센서(120)와 좌표계를 도시한 도면으로, 이를 참고하면, 경사각도(α)는 지자기 센서(120)가 자북으로 향할 때 지구의 수평면과 각도로서 적도 부분에서 지면과 수평을 이루어 "0"이며, 북극에 가까울수록 "90"도에 접근함을 알 수 있다. 경사각도(α)는 지구의 위치에 영향을 받는 각도이나, 수평면에 투영(projection)된 성분은 자북을 향하고, 이 방향은 변하지 않는다. 경사각도(α)는 외란이 존재하지 않는 한 변화하지 않는다. 지질학적으로 지구 자력선이 남극, 자남에서 출발하여 자북으로 향한다. 이는 지구내부의 자성의 변화와 기후 변화에도 영향을 받아 International Association of Geomagnetism and Aeronomy 에서 매 5년 마다 지구 모델을 기초하여 기준이 되는 새로운 지자기 표준을 발표하고 있다. 지자기 강도는 지구 상의 위치에 따라 그 크기가 변하여 약 25 ~ 65μTesla(또는 0.25 ~ 0.65 Gauss)정도의 크기를 갖는다.
도 5는 지자기 센서와 가속도 센서가 동시에 작용하는 좌표계를 도시한 도면으로, 이를 참고하면, 가속도 센서(110)에서 검출된 중력 가속도는 수직 방향을 향하고, 지자기는 수평면과 경사각을 이루며 자북을 향함을 알 수 있다. 가속도 센서(110)에서 선형 가속도가 없는 경우, 중력 가속도는 -z 방향으로 9.8m/sec2가 검출된다.
중력 가속도와 지자기는 기준 좌표계(reference frame, inertial frame: X-Y-Z)에서 검출되는 불변량이다. 그러나 실제의 가속도 센서(110) 및 지자기 센서(120)는 사용자 좌표계(body frame: x-y-z)에서 센서 출력 값을 획득하므로, 좌표계 일치를 위해 사용자 좌표계를 회전시키면, 역으로 회전 각도로부터 사용자 좌표계의 각도를 계산할 수 있다.
다만, 사용자 좌표계는 이동 및 회전하는 특성이 있으나, 본 발명에서는 회전에 의해 발생된 각도 변화만을 산출한다. ω는 임의의 축으로 회전시킬 때의 각속도를 나타내며 기준 좌표계에 대해서 각도 변화를 유발시킨다.
즉, 본 발명은 사용자 좌표계에서 측정된 가속도 센서와 지자기 센서의 2 벡터를 최종적으로 기준 좌표계로 변환하여 가속도 센서와 지자기 센서를 보상하며, 이때에 발생하는 기준 좌표계와 사용자 좌표계의 상대 각도 변화를 기반으로 각속도를 산출하도록 한다.
본 발명에서는 도 6에서와 같은 기준 좌표계와 사용자 좌표계의 상대 각도 변화를 표현하기 위해, 수학식 1 또는 2에서와 같은 쿼터니온(quaternion)표현 방법을 도입하도록 한다.
Figure 112017054692895-pat00082
Figure 112017054692895-pat00083
이때, q0은 쿼터니온에서
Figure 112017054692895-pat00084
스칼라(scalar)에 해당하는 인자이며, q1,q2,q3 또는 qx,qy,qz는
Figure 112017054692895-pat00085
,
Figure 112017054692895-pat00086
,
Figure 112017054692895-pat00087
로써 x, y, z 축의 벡터(vector)에 해당하며, R4는 쿼터니온이 4개의 인자로 구성된 변수임을 표현한다. θ는 오일러(Euler)각이나 찰스(Chales)의 정의에 의한 회전각을 의미하고, i,j,k는 각 축의 단위 벡터를 의미하고, '는 세로 행렬(transpose matrix)을 의미한다.
공간 상의 임의의 한 점 Pn을 기준 좌표계로 표현하고, Pb를 사용자 좌표계로 표현할 수 있으며, Pn과 Pb의 상관 관계를 회전 행렬 C(q)(rotation matrix)로 표현할 수 있다. 이를 쿼터니온으로 표현하면 수학식 3이 된다.
Figure 112017054692895-pat00088
그리고 회전 행렬 C(q)는 수학식 4으로 표현된다. 차후, 수학식 4는 쿼터니온을 산출한 후 회전 행렬로 변환시킬 때 활용된다.
Figure 112017054692895-pat00089
좌표계가 특정 각속도로 회전하며 이를 이용해서 쿼터니온의 미분 값을 수학식5의 운동 방정식으로 표현할 수 있다.
Figure 112017054692895-pat00090
이때,
Figure 112017054692895-pat00091
는 일반 곱(multiply)이 아니라 쿼터니온의 곱으로 기존 곱과는 다르다. 쿼터니온은 4개의 요소, 각속도는 3개의 요소이므로, 다른 방식으로 계산이 수행된다. 수학식 5은 좌표계가 특정 각속도(ω)로 회전할 때 현재 시점의 쿼터니온을 다음 시점의 쿼터니온을 산출할 때 이용하는 수식이다. 수학식 5 수식은, 차후 칼만필터의 추정 단계에서 사용된다.
가속도 센서에서 측정되는 기준 좌표계에서는 중력 가속도
Figure 112017054692895-pat00092
와 지자기 센서에서 측정되는 지구 지자기
Figure 112017054692895-pat00093
를 벡터 형태로 다음의 수학식 6 및 7와 같이 표현될 수 있다.
Figure 112017054692895-pat00094
Figure 112017054692895-pat00095
수학식 7에서, α는 도 1에서의 경사각도(inclination angle)이다. 특히 지자기는 지자기의 강도(intensity)M0가 포함되나, 지자기 강도는 사용자의 위치에 따라 변화하므로 측정값을 무차원화(dimensionless)하여 각도 정보만을 활용한다. 경사각도는 지구의 남반부에서는 수평면의 위쪽으로 향하고 부호가 "-"로 변경된다. 또한 수학식 6에서 중력 가속도를 무차원화하지 않은 이유는 중력 가속도는 위치에 따라 거의 변화하지 않는 상수이므로 식을 전개하는 데에 영향을 미치지 않기 때문이다.
본 발명은 중력 가속도 벡터와 지자기 벡터가 즉, 2 벡터 기준이 되어 공간상 임의의 벡터의 상대 각도를 계산하는 방식이며, 이를 위해서 칼만 필터를 적용한다.
특히, 본 발명은 쿼터니온보다는 선형화에 유리한 에러 쿼터니온
Figure 112017054692895-pat00096
(error quaternion)으로 구성된 간접식 칼만 필터(indirect Kalman filter)방식을 활용한다. 에러 쿼터니온을 수학식 8에서와 같이 정의하고, 이를 상태 방정식의 상태로도 활용하도록 한다.
칼만 필터에서 추정(estimation)하는 물리양은 사용자 좌표계의 회전 정보를 나타내는 쿼터니온이며, 실제로 이 쿼터니온은 시스템의 동적 특성에 기인하여 추정이 가능하다. 추정된 쿼터니온(
Figure 112017054692895-pat00097
)은 오차를 포함할 수 있다. 수학적인 모델 차체에서 기인할 수 있는 오차와 비선형 시스템을 선형화하면서 발생하는 에러 등의 요인에 의해서 쿼터니온(q)을 추정할 때 에러가 반드시 포함된다. 이는 수학식 5의 운동 방정식을 이용하여 추정하여도 불가피하게 발생하는 에러이다. 에러가 포함된 추정 쿼터니온을(
Figure 112017054692895-pat00098
)로 표현하였다. 칼만 필터 적용할 필요한 보상이란 과정을 구현해야 하며, 이 과정은 측정을 통해서 확보할 수 있는 정보인 가속도와 지자기의 값을 이용하여 구현되어야 한다. 쿼터니온 보상을 위해서 사용되는 것이 미소 변화량을 의미하는 쿼터니온 즉, 에러 쿼터니온(
Figure 112017054692895-pat00099
또는
Figure 112017054692895-pat00100
)이다. 수학식 5와 이하의 수학식 8을 이용해서 칼만 필터의 추정과 보상이라는 가장 필수적인 과정을 수행한다.
Figure 112017054692895-pat00101
Figure 112017054692895-pat00102
이때,
Figure 112017054692895-pat00103
는 [qex qey qez]'이다.
본 발명에서 칼만필터를 이용해서 각속도(ω)를 산출하기 위해서 사용하는 가속도 센서(Ya)와 지자기 센서(Ym)는 수학식 10,11와 같은 출력식(또는 출력 모델)으로 정의된다.
Figure 112017054692895-pat00104
Figure 112017054692895-pat00105
수학식 10,11에서 C(q)은 회전 행렬이며 가속도 센서의 출력식인 수학식 10에서 Ba는 선형 가속도이며, 정상 상태(즉, 가속도가 없는 상태)일 때에는 이 성분은 수학식 10에서 나타나지 않는다. Va는 가속도 센서 센서의 잡음이며, 평균값은 "0"이다. Vm는 지자기 센서의 잡음이며 평균값은 역시 "0"이다. 분산값(variance)은 가속도 센서, 지자기 센서의 제조업체마다 차이가 있어, 사용자에게 성능 사양표에 제공된다. Ra는 가속도 센서의 분산이며, Rm은 지자기 센서의 분산이다.
Figure 112017054692895-pat00106
Figure 112017054692895-pat00107
센서의 출력 방정식에서 가속도 센서나 지자기센서의 오프셋은 보상 제거된 것으로 가정한다. 지자기 센서에서 오프셋은 보상이라는 별도의 보상 과정을 통해 제거되어야 한다. 오프셋이 존재하면 이 후에 산출되는 모든 과정에 오차를 유발하기 때문이다. 이런 오프셋은 센서의 고유한 출력 값이 아니라 주변 환경에 영향을 받으므로, 새로운 환경에서 사용될 때 반드시 재보상을 해야 한다.
칼만 필터를 구성하기 위해서는 수학식 5처럼 각속도(ω)를 이용한 운동 방정식을 이용하는 것이 일반적인 방법이나, 본 발명에서는 자이로 센서를 수학적으로 구현하는 것을 목적으로 하므로 자이로 센서의 활용이 불가능하다.
이를 위해서 현재 상태(state)와 이전의 상태(state)가 시간 지연(τ)만 발생하는 통계적 상관관계로 가정한다. 이는 랜덤 프로세스라고 가정할 수 있는 것이며 상태 간의 관계를 지수함수적으로 상관되는 것을 가정하는 것이다. 물리적인 의미로서는 현 시점 상태와 다음 시점 상태가 크게 변화하지 않는 것을 의미한다. 이와 같은 가정이 유효한 것은 사용자의 동작의 동적 거동이 센서의 동특성보다 현저하게 느려 사용자의 동작이외에는 다른 요인들이 영향을 끼치지 않기 때문이다. 이에 휴대 전화와 같은 모바일 장치를 사용자가 사용할 때의 사용자 운동의 동적 특성이 센서들의 동특성에 영향을 주지 못한다. 이 가정을 전제로 다음과 같은 수학식 14의 상태 방정식을 유도할 수 있다.
Figure 112017054692895-pat00108
그리고 F와 G는 다음과 같이 표현된다.
Figure 112017054692895-pat00109
Figure 112017054692895-pat00110
이때, τ는 상태들 간에 연관(correlated)되는 정도를 나타내는 시간 인자로, 상수로 표현 가능하며, 적용 시스템에 따라 실험을 통해서 0.1~0.6 값을 가질 수 있다.
x는 상태(state)이며, F는 전이 행렬(transfer matrix)을, G는 잡음 행렬을, w(t)는 시스템 잡음을 각각 의미한다. 수학식 14는 일반적인 칼만 필터 전이식이므로, 간접 칼만 필터를 적용하기 위해서 상태를 에러 쿼터니온으로 설정하고 이하의 수학식 17로 표현한다. 수학식 17에서 에러 쿼터니온을 상태로 이용할 때 벡터 부분에 해당하는 3 요소만을 선택 적용하는 이유는 스칼라에 해당하는 부분이 "1"이란 상수이어서 변수가 될 수 없기 때문이다.
Figure 112017054692895-pat00111
수학식 14를 이용하여 현재의 상태를 다음 상태로 전이(transfer)시킨다. 칼만 필터의 속성에 의해 상태 변수의 분산도 함께 전이해야 하며, 이는 이하의 수학식 18로 표현된다.
Figure 112017054692895-pat00112
이때, x는 상태 변수이고,
Figure 112017054692895-pat00113
는 추정된 상태 변수이고, '는 세로 행렬(transpose matrix)을 의미한다.
추정된 상태는 수학식 18을 이용하여 보정하며, 보정은 측정값을 이용한다. 에러 쿼터니온을 기반으로 시스템 전이 행렬을 재작성하면 수학식 19에서와 같이 표현될 수 있다.
Figure 112017054692895-pat00114
수학식 18 및 19는 상태 추정 상태이며, 이는 "＾" 부호로 표현된다. 또한 수학식 10, 11의 센서 출력식에서 나타난 회전 행렬 C(q)는 에러 쿼터니온(qe)이 아닌 쿼터니온(q)으로 표현되었기 때문에 수학식 19에 적합한 에러 쿼터니온으로 전개가 필요하다.
이에 수학식 8을 이용하여 회전 행렬을 재작성하면, 수학식 20과 같이 회전 행렬은 추정 쿼터니온 기반의 회전 행렬과 에러 쿼터니온 기반의 회전 행렬의 곱으로 표현되며, 이를 기반으로 수학식 21과 같은 에러 쿼터니온 기반의 회전 행렬을 획득할 수 있게 된다.
Figure 112017054692895-pat00115
Figure 112017054692895-pat00116
이때, qex, qey, qez이 미소량이며 이들의 곱은 더욱 미소량이라는 가정하에 수학식 22에 속하는 항들을 제거할 수 있으며, 이에 따라 수학식 21은 수학식 23에서와 같이 간략화될 수 있다.
Figure 112017054692895-pat00117
Figure 112017054692895-pat00118
이때,
Figure 112017054692895-pat00119
는
Figure 112017054692895-pat00120
를 의미하고 임의의 3x1 또는 3x3 행렬을 곱(multiplication)하는 과정을 간략히 표현하는 부호이다.
또한 식(20)에서의
Figure 112017054692895-pat00121
는 추정된 쿼터니온 기반의 회전행렬로 수학식 23에서와 같이 정리될 수 있다.
Figure 112017054692895-pat00122
최종적으로 회전 행렬을 에러 쿼터니온(qe)기반으로 표현하였으므로, 수학식 19의 상태 방정식에 대입이 가능하다. 수학식 24를 이용하여 센서 출력식을 재정리하면 다음과 같다.
Figure 112017054692895-pat00123
Figure 112017054692895-pat00124
수학식 25, 26을 근거로 칼만 필터의 관측 행렬(observation matrix)을 작성하면 다음과 같다.
Figure 112017054692895-pat00125
그리고 칼만 필터의 출력 행렬(output matrix)은 수학식 28로 표현된다.
Figure 112017054692895-pat00126
본 발명을 실제 제품을 적용하기 위해서는 이산화(discretization)과정이 필요하다. 전이 행렬을 이산화 시킬 때 샘플링 시간은 T로 표현된다. 이산화된 전이 행렬을 이용하여 에러 쿼터니온(즉, 상태 변수와 상태변수의 분산)도 전이되므로, 상태 변수와 상태변수의 분산의 전이식은 수학식 29, 30으로 표현된다.
참고로, 수학식 14, 19에선 Gw(t)를 포함하고 있으나, 수학식 29는 추정 값을 전이시키는 과정에 대한 식으로, w(t)는 평균값이 "0"인 잡음이므로 추정값으로 전개되는 수학식 29식에서는 Gw(t)가 소거된다.
Figure 112017054692895-pat00127
Figure 112017054692895-pat00128
이때, Fk는 이산화된 전이 행렬이며, T는 샘플링 시간이다
Figure 112017054692895-pat00129
또한, Qdk는 Q의 이산화된 프로세스 잡음 분산으로 수학식 32와 같이 표현된다. 적분요소가 포함된 식은 1차 선형식으로 가정하여, 수학식 31,32를 보다 간략화하면 수학식 32,33과 같이 표현된다.
Figure 112017054692895-pat00130
Figure 112017054692895-pat00131
Figure 112017054692895-pat00132
이러한 수학식들을 이용하여 칼만 필터의 이득을 계산하면, 수학식 35로 표현된다.
Figure 112017054692895-pat00133
수학식 35에서 행렬 R은 센서의 측정 행렬 분산, 또는 측정 센서 에러의 분산이며 앞서 설명된 수학식 12, 13을 이용하여 이하의 수학식 36으로 정의될 수 있다.
Figure 112017054692895-pat00134
이때, I3×3은 3X3 단위 행렬이며, 03×3은 3X3 0행렬이다.
그리고 칼만 필터의 이득을 이용하여 상태변수와 상태변수의 분산을 보상하면, 보상된 상태변수와 상태변수의 분산은 수학식 37, 38에서와 같이 표현된다.
Figure 112017054692895-pat00135
Figure 112017054692895-pat00136
수학식 37, 38에서 보상이 이루어지면 더 이상 추정 변수가 아니므로 "＾"은 소멸되는 것을 확인 할 수 있다.
보상된 상태변수 xk는 에러 쿼터니온(qe)으로, 수학식 39에서와 같이 에러 쿼터니온(qe)과 추정 쿼터니온(
Figure 112017054692895-pat00137
)을 곱하여, 쿼터니온(q)을 구하게 된다. 다만, 쿼터니온(q)은 4인자 변수인 반면, 에러 쿼터니온(qe)은 3인자 변수이므로, 에러 쿼터니온(qe)에 "1"이란 상수를 스칼라에 해당하는 인자에 추가하도록 한다.
Figure 112017054692895-pat00138
이와 같은 산출된 쿼터니온에서 각속도(
Figure 112017054692895-pat00139
)를 산출하는 과정은 다음과 같다.
먼저, 쿼터니온을 미분을 하면 이하의 수학식 40과 같이 표현된다.
Figure 112017054692895-pat00140
이때, W(t)는 수학식 5에서처럼 각속도 벡터를 의미하나, 쿼터니온은 4인자 변수이고,
Figure 112017054692895-pat00141
는 3인자 변수이므로, 연산을 위해서
Figure 112017054692895-pat00142
로 조정된 변수이다.
수학식 40을 인자별로 정리하면 다음과 같다.
Figure 112017054692895-pat00143
이러한 수학식 41은 수학식 42로 정리될 수 있다.
Figure 112017054692895-pat00144
이때,
Figure 112017054692895-pat00145
는 정의에 의해서
Figure 112017054692895-pat00146
이다.
수학식 41과 수학식 42를 이산화하여 수학식 43과 같은
Figure 112017054692895-pat00147
이 산출된다. 이때, 인덱스 k-1은 이산화된 변수의 이전 단계를 의미한다.
Figure 112017054692895-pat00148
그러면, 수학식 43을 기반으로 자이로 센서에서 측정할 수 있는 각속도를 가속도 센서와 지자기 센서를 이용하여 산출할 수 있게 된다.
도 7은 본 발명의 다른 실시예에 따른 가속도 센서와 지자기 센서 기반의 각속도 산출 방법을 동작 흐름도이다.
도 7에 도시된 바와같이, 본 발명의 다른 실시예에 따른 각속도 산출 방법은 앞서 설명된 쿼터니온을 이용하여 오일러 각도를 산출하는 단계(S21), 쿼터니온을 이용하여 오일러 회전 축과 각도를 산출하는 단계(S22), 쿼터니온을 이용하여 선형 가속도를 산출하는 단계(S23), 및 쿼터니온을 이용하여 회전 행렬을 산출하는 단계(S24)중 적어도 하나를 더 포함할 수 있다.
앞서 설명된 쿼터니온은 다른 형태 출력들로 다양하게 변환이 가능하다.
먼저, 오일러 각도가 쿼터니온을 이용하여 이하의 수학식 44, 45, 46와 같이 표현될 수 있다. 축을 회전시키는 순서에 따라 다르게 표현이 되어, 12가지 조합의 오일러 각도를 표현할 수 있다. 본 발명은 그 대표적인 예로서 z축, y축, x 축(3-2-1)순서로 회전 시키는 예를 들었다.
Figure 112017054692895-pat00149
Figure 112017054692895-pat00150
Figure 112017054692895-pat00151
그리고 오일러 축(
Figure 112017054692895-pat00152
)과 각도(
Figure 112017054692895-pat00153
)는 쿼터니온을 이용하여 수학식 47, 48과 같이 표현될 수 있다.
Figure 112017054692895-pat00154
Figure 112017054692895-pat00155
이때,
Figure 112017054692895-pat00156
로서 쿼터니온의 벡터에 해당하는 요소이고, i,j,k는 각 축의 단위 벡터이고,
Figure 112017054692895-pat00157
는 벡터
Figure 112017054692895-pat00158
의 절대값을 의미한다.
그리고 수학식 10에서 Ya는 가속도 센서에서 측정한 출력값이고
Figure 112017054692895-pat00159
는 중력 가속도에 해당하므로, 운동 중의 대상체의 선형 가속도 또한 수학식 49와 같이 쿼터니온을 이용하여 산출할 수 있다.
Figure 112017054692895-pat00160
뿐 만 아니라, 쿼터니온을 이용하여 앞서 설명된 수학식 4에서와 같은 회전 행렬 또한 산출할 수 있을 것이다.
도 8 내지 도 10은 본 발명의 가속도 센서와 지자기 센서 기반의 각속도 산출 방법의 성능을 설명하기 위한 도면이다.
도 8은 지자기 센서의 보정 및 방위각 측정 시, 가속도 센서와 지자기센서의 실제 측정값을 비교한 도면이다. 지자기 센서의 보정 및 방위각 측정 시, 가속도 센서와 지자기센서는 수평을 이루어 구동되므로, 가속도 센서 신호에서 회전 운동에 의한 미미한 회전 가속도가 검출되나 선형 가속도는 발견되지 않음을 알 수 있다.
도 9는 자이로 센서의 실측값과 본 발명에 따른 각속도 산출값을 비교한 도면이다.
도 9를 참고하면, 자이로 센서의 출력은 z 축에서만 회전 운동이 측정되는 것을 관찰할 수 있다. 그러나 각속도 산출값은 z축만 아니라 x, y축에서 잡음 신호가 포함되어 있다. 이는 쿼터니온의 미분 산출 과정에서 발생하는 잡음이 섞여 있기 때문이다. 샘플링 시간이 짧을수록 그 잡음의 정도가 크다. 그러나 이 잡음은 램덤(random)한 잡음으로, 그 평균값이 "0"이므로 에버리지(average)필터 등을 적용하여 용이하게 제거할 수 있으며, 시간이 지나면 나타나는 포화 현상을 회피할 수 있다.
도 10은 자이로 센서의 z 축 값과 산출된 각속도의 z축 값을 비교한 도면이다. 도10을 참고하면, 산출된 각속도의 z축의 신호에서 시간 지연과 잡음이 관찰되나, 자이로 센서의 z 축 값의 기본적인 형태를 충실하게 추종하고 있음을 확인할 수 있다. 이러한 잡음은 에버리지(average)필터 등을 통해 제거할 수 있으며, 시간 지연 정도는 수학식 15의 τ를 조정하여 튜닝 가능하다.
상술한 실시예에 설명된 특징, 구조, 효과 등은 본 발명의 적어도 하나의 실시예에 포함되며, 반드시 하나의 실시예에만 한정되는 것은 아니다. 나아가, 각 실시예에서 예시된 특징, 구조, 효과 등은 실시예들이 속하는 분야의 통상의 지식을 가지는 자에 의하여 다른 실시예들에 대해서도 조합 또는 변형되어 실시 가능하다.
따라서 이러한 조합과 변형에 관계된 내용들은 본 발명의 범위에 포함되는 것으로 해석되어야 할 것이다. 또한, 이상에서 실시예들을 중심으로 설명하였으나 이는 단지 예시일 뿐 본 발명을 한정하는 것이 아니며, 본 발명이 속하는 분야의 통상의 지식을 가진 자라면 본 실시예의 본질적인 특성을 벗어나지 않는 범위에서 이상에 예시되지 않은 여러 가지의 변형과 응용이 가능함을 알 수 있을 것이다. 예를 들어, 실시예들에 구체적으로 나타난 각 구성 요소는 변형하여 실시할 수 있는 것이다. 그리고 이러한 변형과 응용에 관계된 차이점들은 첨부한 청구 범위에서 규정하는 본 발명의 범위에 포함되는 것으로 해석되어야 할 것이다.

Claims (27)

  1. 에러 쿼터니온을 기반으로 상태 변수와 상태 변수의 분산을 도출하고, 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계;
    상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도 센서 출력과 지자기 센서의 출력을 재정의한 후, 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계;
    이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시킨 후, 상기 전이된 상태 변수의 분산과 상기 칼만 필터의 관측 행렬을 기반으로 상기 칼만 필터의 이득을 산출하는 단계;
    상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상한 후, 상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 단계; 및
    상기 쿼터니온을 기반으로 각속도를 산출하는 단계; 포함하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  2. 제1항에 있어서, 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는
    기준 좌표계와 센서 좌표계간의 상대 각도 변화를 표현하는 쿼터니온(q=[q0 qx qy qz]')의 에러 쿼터니온([qex qey qez]')을 상태 변수(x)로 설정하며 상기 ´는 세로 행렬(transpose matrix)이고 상기 qex, qey, qez는 상기 에러 쿼터니온의 벡터 성분 인자인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  3. 제2항에 있어서, 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는
    현재 상태(state)와 이전의 상태(state)가 시간 지연(τ)만 발생하는 통계적 상관관계인 것이라는 가정하에 상기 상태 변수(x)를 "
    Figure 112017054692895-pat00161
    "의 식으로 표현하며, 상기 F는 "
    Figure 112017054692895-pat00162
    "으로 표현되는 전이 행렬이며, 상기 τ는 상태들 간에 연관되는 정도를 나타내는 시간 인자인 것으로 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  4. 제1항에 있어서, 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는
    상기 상태 변수의 분산을 "
    Figure 112017054692895-pat00163
    "의 식에 따라 산출하며, 상기 x는 상태 변수이고, 상기
    Figure 112017054692895-pat00164
    는 추정된 상태 변수이고, 상기 '는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  5. 제1항에 있어서, 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는
    "
    Figure 112018069579076-pat00252
    "의 식을 기반으로 상기 쿼터니온 기반의 회전 행렬을 상기 에러 쿼터니온 기반의 회전 행렬로 변환하며,
    상기 쿼터니온 기반의 회전 행렬은 "
    Figure 112018069579076-pat00253
    "이고, 상기 에러 쿼터니온 기반의 회전 행렬은 "
    Figure 112018069579076-pat00254
    "이며, 상기
    Figure 112018069579076-pat00255
    는 상기 에러 쿼터니온이고, 상기 qex , qey , qez는 상기 에러 쿼터니온의 벡터 성분 인자이고, 상기
    Figure 112018069579076-pat00256
    는
    Figure 112018069579076-pat00257
    를 의미하고, 상기 q0은 쿼터니온에서
    Figure 112018069579076-pat00258
    스칼라(scalar)에 해당하는 인자인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  6. 제5항에 있어서, 상기 에러 쿼터니온 기반의 회전 행렬로 변환하는 단계는
    상기 qex, 상기 qey, 상기 qez이 미소량이라는 가정하에, "
    Figure 112018069579076-pat00259
    "으로 표현되는 상기 에러 쿼터니온 기반의 회전 행렬을 "
    Figure 112018069579076-pat00260
    "으로 간략화하고 상기 I3×3은 3×3 단위 행렬인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  7. 제1항에 있어서, 상기 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계는
    "
    Figure 112017054692895-pat00173
    "으로 표현되는 상기 에러 쿼터니온(qe)과 쿼터니온 기반의 회전 행렬(C(q))간 상관관계를 기반으로 가속도 센서 출력(Ya)및 지자기 센서의 출력(Ym)을 재정의하며,
    상기 가속도 센서 출력(Ya)은 "
    Figure 112017054692895-pat00174
    "이고, 상기 지자기 센서의 출력(Ym)은 "
    Figure 112017054692895-pat00175
    "이며, 상기
    Figure 112017054692895-pat00176
    은 추정된 쿼터니온 기반의 회전 행렬이고, 상기 I3×3은 3×3 단위 행렬, 상기
    Figure 112017054692895-pat00177
    는 중력 가속도 벡터이고, 상기 Ba는 선형 가속도이고, 상기 Va는 가속도 센서 센서의 잡음이고, 상기
    Figure 112017054692895-pat00178
    는 지구 지자기 벡터이고, 상기 Vm은 지자기 센서의 잡음인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  8. 제7항에 있어서, 상기 칼만 필터의 관측 행렬 및 출력 행렬을 산출하는 단계는
    상기 가속도 센서 출력(Ya)과 상기 지자기 센서의 출력(Ym)을 기반으로 "
    Figure 112017054692895-pat00179
    "의 칼만 필터의 관측 행렬을 산출하고, "
    Figure 112017054692895-pat00180
    "의 상기 칼만 필터의 출력 행렬을 산출하는 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  9. 제1항에 있어서, 상기 칼만 필터의 이득을 산출하는 단계는
    "
    Figure 112018069579076-pat00261
    "의 식에 따라 상기 상태 변수(x)를 전이하고,
    Figure 112018069579076-pat00262
    "의 식에 따라 상기 상태 변수의 분산(p)을 전이하며,
    상기 Fk는 "
    Figure 112018069579076-pat00263
    "의 식으로 표현되는 이산화된 전이 행렬이고, 상기 Qdk는 "
    Figure 112018069579076-pat00264
    "의 식으로 표현되는 Q의 이산화된 프로세스 잡음 분산이고, 상기 ´는 세로 행렬(transpose matrix)이며 상기 T는 샘플링 시간인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  10. 제9항에 있어서, 상기 칼만 필터의 이득을 산출하는 단계는
    "
    Figure 112017054692895-pat00185
    "의 식으로 표현되는 이산화된 전이 행렬을 1차 선형식으로 가정하여 "
    Figure 112017054692895-pat00186
    "으로 간략화하며, 상기 F는 "
    Figure 112017054692895-pat00187
    "로 표현되는 전이 행렬이고, 상기 τ는 상태들 간에 연관되는 정도를 나타내는 시간 인자인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  11. 제9항에 있어서, 상기 칼만 필터의 이득을 산출하는 단계는
    "
    Figure 112018069579076-pat00265
    "의 식에 따라 상기 칼만 필터의 이득을 산출하며,
    상기 R은 측정 센서 에러의 분산으로 "
    Figure 112018069579076-pat00266
    "의 식으로 표현되며, 상기 ´는 세로 행렬(transpose matrix)이고, 상기 I3×3은 3×3 단위 행렬이고, 상기 03×3은 3×3 0 행렬이고, 상기 Ra는 가속도 센서의 분산이고, 상기 Rm는 지자기 센서의 분산인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  12. 제1항에 있어서, 상기 쿼터니온을 산출하는 단계는
    "
    Figure 112018069579076-pat00267
    "의 식에 따라 상기 상태 변수를 보상하고, "
    Figure 112018069579076-pat00268
    "의 식에 따라 상기 상태 변수의 분산을 보상하며,
    상기 Kk는 칼만 필터의 이득, 상기 Zk는 칼만 필터의 출력 행렬, 상기 Hk는 칼만 필터의 관측 행렬, 상기 ´는 세로 행렬(transpose matrix), 상기 I은 3×3 단위 행렬, 상기 R은 측정 센서 에러의 분산인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  13. 제12항에 있어서, 상기 쿼터니온을 산출하는 단계는
    상기 보상된 상태 변수를 에러 쿼터니온으로 설정한 후, "
    Figure 112018069579076-pat00269
    "의 식에 따라 쿼터니온을 산출하며,
    상기
    Figure 112018069579076-pat00270
    는 추정된 쿼터니온, 상기 qex, qey, qez는 에러 쿼터니온의 3인자 변수, 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  14. 제1항에 있어서, 상기 쿼터니온을 기반으로 각속도를 산출하는 단계는
    상기 쿼터니온을 미분하여 "
    Figure 112018069579076-pat00271
    "의 식으로 표현되는 미분식을 획득한 후, 상기 미분식을 인자별로 정리한 후 이산화하여 "
    Figure 112018069579076-pat00272
    "의 식으로 표현되는 각속도 산출식을 획득하며,
    상기 W(t)는 "
    Figure 112018069579076-pat00273
    "의 식으로 표현되는 각속도 벡터, 상기 q0,qx,qy,qz는 상기 쿼터니온을 구성하는 4개의 인자, 상기 T는 샘플링 시간, 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  15. 제14항에 있어서, 상기 쿼터니온을 기반으로 각속도를 산출하는 단계는
    상기 미분식을 인자별로 정리하여 "
    Figure 112018069579076-pat00274
    ", "
    Figure 112018069579076-pat00275
    ", "
    Figure 112018069579076-pat00276
    ", "
    Figure 112018069579076-pat00277
    "의 식을 도출 및 정리하여 "
    Figure 112018069579076-pat00278
    "의 식으로 표현되는 각속도 벡터 산출식을 생성한 후, 상기 각속도 벡터 산출식을 이산화하여 상기 각속도 산출식을 획득하며,
    상기 q-1는 "
    Figure 112018069579076-pat00279
    "의 식으로 표현되고 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  16. 제1항에 있어서,
    상기 쿼터니온에서 오일러 각도를 z축 y축 x축 순서로 회전시켜 산출하는 단계를 더 포함하며,
    상기 오일러 각도는 "
    Figure 112017054692895-pat00203
    ", "
    Figure 112017054692895-pat00204
    ", "
    Figure 112017054692895-pat00205
    " 중 적어도 하나의 식을 기반으로 산출되며, 상기 q0,qx,qy,qz는 상기 쿼터니온을 구성하는 4개의 인자인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  17. 제1항에 있어서,
    상기 쿼터니온을 기반으로 오일러 회전 축과 각도를 산출하는 단계를 더 포함하며,
    상기 오일러 회전 축은 "
    Figure 112017054692895-pat00206
    "의 식에 따라 산출되며, 상기 오일러 회전 축 각도는 "
    Figure 112017054692895-pat00207
    "의 식에 따라 산출되며, 상기
    Figure 112017054692895-pat00208
    는 "
    Figure 112017054692895-pat00209
    "으로 표현되는 쿼터니온의 벡터, 상기 i,j,k는 각 축의 단위 벡터, 상기
    Figure 112017054692895-pat00210
    는 상기
    Figure 112017054692895-pat00211
    의 절대값인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  18. 제1항에 있어서,
    상기 쿼터니온을 기반으로 운동 중의 대상체의 선형 가속도를 산출하는 단계를 더 포함하며,
    상기 선형 가속도는 "
    Figure 112017054692895-pat00212
    "의 식에 따라 산출되며, 상기 Ya는 가속도 센서 출력이며, 상기 C(q)는 쿼터니온, 상기
    Figure 112017054692895-pat00213
    는 중력 가속도 벡터인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 방법.
  19. 에러 쿼터니온을 설정하는 에러 쿼터니온 설정부;
    상기 에러 쿼터니온을 기반으로 상태 변수, 상태 변수의 분산을 도출하고, 쿼터니온 기반의 회전 행렬을 에러 쿼터니온 기반의 회전 행렬로 변환하는 예측부;
    상기 에러 쿼터니온 기반의 회전 행렬을 이용하여 가속도 센서 출력과 지자기 센서의 출력을 재정의하여 칼만 필터의 관측 행렬 및 출력 행렬을 산출하고, 이산화된 전이 행렬을 통해 상기 상태 변수와 상기 상태 변수의 분산을 전이시켜 상기 칼만 필터의 이득을 산출하는 필터 구성부;
    상기 칼만 필터의 이득을 통해 상기 상태 변수와 상기 상태 변수의 분산을 보상하는 보상부;
    상기 보상된 상태 변수와 추정된 쿼터니온을 기반으로 쿼터니온을 산출하는 쿼터니온 산출부; 및
    상기 쿼터니온을 기반으로 각속도를 산출하는 각속도 획득부를 포함하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  20. 제19항에 있어서, 상기 예측부는
    기준 좌표계와 센서 좌표계간의 상대 각도 변화를 표현하는 쿼터니온(q=[q0 qx qy qz]')의 에러 쿼터니온([qex qey qez]')을 상태 변수(x)로 설정하고,
    현재 상태(state)와 이전의 상태(state)가 시간 지연(τ)만 발생하는 통계적 상관관계인 것이라는 가정하에 상기 상태 변수(x)를 "
    Figure 112018069579076-pat00280
    "의 식으로 표현하며, 상기 ´는 세로 행렬(transpose matrix), 상기 qex, qey 및 qez는 상기 에러 쿼터니온의 벡터 성분 인자이고, 상기 F는 "
    Figure 112018069579076-pat00281
    "으로 표현되는 전이 행렬이며, 상기 τ는 상태들 간에 연관되는 정도를 나타내는 시간 인자인 것으로 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  21. 제19항에 있어서, 상기 예측부는
    "
    Figure 112018069579076-pat00282
    "의 식을 기반으로 상기 쿼터니온 기반의 회전 행렬을 상기 에러 쿼터니온 기반의 회전 행렬로 변환하며,
    상기 쿼터니온 기반의 회전 행렬은 "
    Figure 112018069579076-pat00283
    "이고, 상기 에러 쿼터니온 기반의 회전 행렬은 "
    Figure 112018069579076-pat00284
    "이며, 상기
    Figure 112018069579076-pat00285
    는 상기 에러 쿼터니온이고 상기 I3×3은 3×3 단위 행렬이며 상기 qex , qey , qez는 상기 에러 쿼터니온의 벡터 성분 인자이고, 상기
    Figure 112018069579076-pat00286
    는
    Figure 112018069579076-pat00287
    를 의미하고, 상기 q0은 쿼터니온에서
    Figure 112018069579076-pat00288
    스칼라(scalar)에 해당하는 인자인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  22. 제19항에 있어서, 상기 필터 구성부는
    "
    Figure 112017054692895-pat00222
    "으로 표현되는 상기 에러 쿼터니온(qe)과 쿼터니온 기반의 회전 행렬(C(q))간 상관관계를 기반으로 가속도 센서 출력(Ya)및 지자기 센서의 출력(Ym)을 재정의하며,
    상기 재정의 된 가속도 센서 출력(Ya)은 "
    Figure 112017054692895-pat00223
    "이고, 상기 재정의 된 지자기 센서 출력(Ym)은 "
    Figure 112017054692895-pat00224
    "이며, 상기
    Figure 112017054692895-pat00225
    은 추정된 쿼터니온 기반의 회전 행렬이고, 상기 I3×3은 3×3 단위 행렬, 상기
    Figure 112017054692895-pat00226
    는 중력 가속도 벡터이고, 상기 Ba는 선형 가속도이고, 상기 Va는 가속도 센서 센서의 잡음이고, 상기
    Figure 112017054692895-pat00227
    는 지구 지자기 벡터이고, 상기 Vm은 지자기 센서의 잡음인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  23. 제22항에 있어서, 상기 필터 구성부는
    상기 가속도 센서 출력(Ya)과 상기 지자기 센서의 출력(Ym)을 기반으로 "
    Figure 112017054692895-pat00228
    "의 칼만 필터의 관측 행렬을 산출하고, "
    Figure 112017054692895-pat00229
    "의 상기 칼만 필터의 출력 행렬을 산출하는 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  24. 제19항에 있어서, 상기 필터 구성부는
    "
    Figure 112018069579076-pat00289
    "의 식에 따라 상기 상태 변수(x)를 전이하고,
    Figure 112018069579076-pat00290
    "의 식에 따라 상기 상태 변수의 분산(p)을 전이한 후, "
    Figure 112018069579076-pat00291
    "의 식에 따라 상기 칼만 필터의 이득을 산출하며,
    상기 Fk는 "
    Figure 112018069579076-pat00292
    "의 식으로 표현되는 이산화된 전이 행렬이고, 상기 Qdk는 "
    Figure 112018069579076-pat00293
    "의 식으로 표현되는 Q의 이산화된 프로세스 잡음 분산이고, 상기 T는 샘플링 시간이고, 상기 Hk는 칼만 필터의 관측 행렬이고, 상기 R은 측정 센서 에러의 분산이고, 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  25. 제19항에 있어서, 상기 보상부는
    "
    Figure 112018069579076-pat00294
    "의 식에 따라 상기 상태 변수를 보상하고, "
    Figure 112018069579076-pat00295
    "의 식에 따라 상기 상태 변수의 분산을 보상하며,
    상기 Kk는 칼만 필터의 이득, 상기 Zk는 칼만 필터의 출력 행렬, 상기 Hk는 칼만 필터의 관측 행렬, 상기 I은 3×3 단위 행렬, 상기 R은 측정 센서 에러의 분산이고, 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  26. 제19항에 있어서, 상기 쿼터니온 산출부는
    상기 보상된 상태 변수를 에러 쿼터니온으로 설정한 후, "
    Figure 112018069579076-pat00296
    "의 식에 따라 쿼터니온을 산출하며,
    상기
    Figure 112018069579076-pat00297
    는 추정된 쿼터니온, 상기 qex, qey, qez는 에러 쿼터니온의 3인자 변수, 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
  27. 제19항에 있어서, 상기 각속도 획득부는
    상기 쿼터니온을 미분하여 "
    Figure 112018069579076-pat00298
    "의 식으로 표현되는 미분식을 획득한 후, 상기 미분식을 인자별로 정리한 후 이산화하여 "
    Figure 112018069579076-pat00299
    "의 식으로 표현되는 각속도 산출식을 획득하며,
    상기 W(t)는 "
    Figure 112018069579076-pat00300
    "의 식으로 표현되는 각속도 벡터, 상기 q0,qx,qy,qz는 상기 쿼터니온을 구성하는 4개의 인자, 상기 T는 샘플링 시간, 상기 ´는 세로 행렬(transpose matrix)인 것을 특징으로 하는 가속도 센서와 지자기 센서 기반의 각속도 산출 장치.
KR1020170071721A 2017-06-08 2017-06-08 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치 KR101922700B1 (ko)

Priority Applications (3)

Application Number Priority Date Filing Date Title
KR1020170071721A KR101922700B1 (ko) 2017-06-08 2017-06-08 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치
US15/984,923 US10788324B2 (en) 2017-06-08 2018-05-21 Method and apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor
CN201810585981.9A CN109030867B (zh) 2017-06-08 2018-06-08 使用加速度传感器和地磁传感器计算角速度的方法和设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
KR1020170071721A KR101922700B1 (ko) 2017-06-08 2017-06-08 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치

Publications (1)

Publication Number Publication Date
KR101922700B1 true KR101922700B1 (ko) 2018-11-27

Family

ID=64563971

Family Applications (1)

Application Number Title Priority Date Filing Date
KR1020170071721A KR101922700B1 (ko) 2017-06-08 2017-06-08 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치

Country Status (3)

Country Link
US (1) US10788324B2 (ko)
KR (1) KR101922700B1 (ko)
CN (1) CN109030867B (ko)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7025215B2 (ja) * 2018-01-05 2022-02-24 ローム株式会社 測位システム及び測位方法
CN110260869B (zh) * 2019-05-10 2023-04-25 哈尔滨工业大学 一种降低星敏感器和陀螺联合滤波计算量的改进方法
CN110579624A (zh) * 2019-06-28 2019-12-17 中国船舶重工集团公司第七0七研究所 从mems加速度计组合中提取载体角速度的方法
CN110497245B (zh) * 2019-08-12 2021-04-16 大连理工大学 基于空间频率连续性原则的直线轴运动误差检测方法
CN110765717A (zh) * 2019-10-22 2020-02-07 哈尔滨理工大学 一种基于fpga的扩展卡尔曼滤波器电路结构设计方法
CN110794969B (zh) * 2019-10-30 2023-05-23 华南理工大学 面向非接触力反馈的自然人机交互方法
CN111207734B (zh) * 2020-01-16 2022-01-07 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN114323007A (zh) * 2021-12-30 2022-04-12 西人马帝言(北京)科技有限公司 一种载体运动状态估计方法及装置

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090089001A1 (en) 2007-08-14 2009-04-02 American Gnc Corporation Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)

Family Cites Families (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6647352B1 (en) * 1998-06-05 2003-11-11 Crossbow Technology Dynamic attitude measurement method and apparatus
US6377906B1 (en) * 2000-02-03 2002-04-23 Independence Technology, L.L.C. Attitude estimation in tiltable body using modified quaternion data representation
US6863244B2 (en) * 2003-01-24 2005-03-08 The Boeing Company Mitigation of angular acceleration effects on optical sensor data
JP2007040763A (ja) * 2005-08-01 2007-02-15 Toyota Motor Corp 加速度センサの補正装置
JP2007041733A (ja) * 2005-08-01 2007-02-15 Toyota Motor Corp 運動体の姿勢角検出装置
US8757548B2 (en) * 2007-04-30 2014-06-24 The Boeing Company Apparatus for an automated aerial refueling boom using multiple types of sensors
JP5072100B2 (ja) * 2008-03-25 2012-11-14 Ｋｄｄｉ株式会社 加速度センサ及び地磁気センサを用いて歩行者の進行方向を決定する携帯端末、プログラム及び方法
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿***惯性/地磁组合方法
CN106342284B (zh) * 2008-08-18 2011-11-23 西北工业大学 一种飞行载体姿态确定方法
CN101726295B (zh) * 2008-10-24 2011-09-07 中国科学院自动化研究所 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
TW201035581A (en) * 2009-03-27 2010-10-01 Ind Tech Res Inst Space detecting apparatus, vehicle and control method thereof
JP2013064695A (ja) * 2011-09-20 2013-04-11 Yamaha Corp 状態推定装置、オフセット更新方法およびオフセット更新プログラム
CN102607562B (zh) * 2012-04-12 2014-10-29 南京航空航天大学 基于载体飞行模态判别的微惯性参数自适应姿态确定方法
US8928309B2 (en) * 2012-05-31 2015-01-06 Blackberry Limited System and method for operating a mobile device having a magnetometer using error indicators
US20140288873A1 (en) * 2013-03-22 2014-09-25 Qualcomm Incorporated Pre-processing inertial sensor measurements for navigation
CN104407709B (zh) * 2014-12-09 2015-12-02 北京银河润泰科技有限公司 可穿戴设备的穿戴状态的处理方法及装置
US10706818B2 (en) * 2015-02-16 2020-07-07 Invensense, Inc. System and method for aligning sensor data to screen refresh rate
JP6474128B2 (ja) * 2015-03-18 2019-02-27 アルプスアルパイン株式会社 地磁気センサと加速度センサを搭載した電子機器
US9846040B2 (en) * 2015-05-08 2017-12-19 Sharp Laboratories Of America, Inc. System and method for determining the orientation of an inertial measurement unit (IMU)
KR101754634B1 (ko) 2015-05-12 2017-07-07 주식회사 신성씨앤티 2자유도 감지 모드를 갖는 멤스 자이로스코프
US20170299388A9 (en) * 2015-05-22 2017-10-19 InvenSense, Incorporated Systems and methods for synthetic sensor signal generation
CN106597017B (zh) * 2016-12-16 2019-07-26 上海拓攻机器人有限公司 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置
US10955261B2 (en) * 2017-04-17 2021-03-23 Rosemount Aerospace Inc. Air data attitude reference system

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090089001A1 (en) 2007-08-14 2009-04-02 American Gnc Corporation Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111426318A (zh) * 2020-04-22 2020-07-17 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN111426318B (zh) * 2020-04-22 2024-01-26 中北大学 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法

Also Published As

Publication number Publication date
US10788324B2 (en) 2020-09-29
US20180356227A1 (en) 2018-12-13
CN109030867A (zh) 2018-12-18
CN109030867B (zh) 2020-12-01

Similar Documents

Publication Publication Date Title
KR101922700B1 (ko) 가속도 센서와 지자기 센서 기반의 각속도 산출 방법 및 장치
US9261980B2 (en) Motion capture pointer with data fusion
Sun et al. MEMS-based rotary strapdown inertial navigation system
CN106052685B (zh) 一种两级分离融合的姿态和航向估计方法
US8326533B2 (en) Apparatus and methodology for calibration of a gyroscope and a compass included in a handheld device
US20110307213A1 (en) System and method of sensing attitude and angular rate using a magnetic field sensor and accelerometer for portable electronic devices
US20140222369A1 (en) Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method
JP2017207456A (ja) 姿勢推定装置、姿勢推定方法、制御プログラム、および記録媒体
CN102937450B (zh) 一种基于陀螺测量信息的相对姿态确定方法
CN106813679B (zh) 运动物体的姿态估计的方法及装置
CN112798021B (zh) 基于激光多普勒测速仪的惯导***行进间初始对准方法
US11408735B2 (en) Positioning system and positioning method
CN109764870B (zh) 基于变换估计量建模方案的载体初始航向估算方法
Sabet et al. Experimental analysis of a low-cost dead reckoning navigation system for a land vehicle using a robust AHRS
CN111076722A (zh) 基于自适应的四元数的姿态估计方法及装置
Li et al. Common frame based unscented quaternion estimator for inertial-integrated navigation
Li et al. Low-cost MEMS sensor-based attitude determination system by integration of magnetometers and GPS: A real-data test and performance evaluation
Zhou et al. A novel adaptive Kalman filter for Euler-angle-based MEMS IMU/magnetometer attitude estimation
JP5678748B2 (ja) 端末装置及び地磁気環境判定プログラム
CN105021193A (zh) 一种无陀螺仪惯性导航***的控制算法
JP2007163388A (ja) 方位センサおよび記録媒体
US11150090B2 (en) Machine learning zero-rate level calibration
JP2013061309A (ja) カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム
CN110375773B (zh) Mems惯导***姿态初始化方法
Chen et al. High-precision geomagnetic directional technology based on sensor error correction and adaptive hybrid filter

Legal Events

Date Code Title Description
E701 Decision to grant or registration of patent right
GRNT Written decision to grant