JP4145451B2 - Hybrid navigation and its device - Google Patents

Description

【００２０】
〔数１〕において、ＩＮＳの位置誤差の分散Ｑ(t) は、シューラ周期８４．４分の１／４周期以上の時間が経過すると、時間に比例する項が振動項よりも優勢になり、１シューラ周期以降は、時間に比例する項が振動項の５倍以上になって、振動項が無視できる。すなわち、１シューラ周期以降は、ＩＮＳの位置誤差の分散は時間に略比例して大きくなっていく。
【００２１】
このことは、ＩＮＳの位置誤差がランジュバン（Langevin）方程式でモデル化できることを示している。
【００２２】
ＩＮＳの誤差を状態変数とするベクトルを〔数２〕で表し、ランジュバン方程式でモデル化すると、Ａをシステム行列、ｕをシステム雑音としてシステム方程式は〔数３〕で表される。ただし、Ｔは転置を示す。
【００２３】
【数２】

【００２４】
【数３】

【００２５】
このように、ＩＮＳの位置誤差をランジュバン方程式でモデル化することにより、システム方程式は極めて単純化できた。相関時定数τ₁ と北方向および東方向の加速度誤差ｕ_n ＝ｕ_e はシステム方程式の設計要素であり、システムの設計により定まる。なお、〔数３〕でＸ′と示したように、微分を意味する′（ドット）付き記号を以下２文字で表す。
【００２６】
〔２〕 観測方程式
観測値ベクトルを（ＩＮＳの位置）−（ＧＰＳの位置）とすると、観測方程式は〔数４〕で表すことができる。
【００２７】
【数４】

【００２８】
前述のように、ＧＰＳ受信機の測位誤差は、受信信号に含まれるマルコフ性の電離層遅れにより、また、受信信号に含まれる擬似雑音符号を位相追尾して測定位置を更新する処理等を行うことにより、自己相関を持つ有色雑音になり、したがって、観測方程式に含まれる観測雑音は有色雑音となってしまう。
【００２９】
観測雑音が有色雑音である場合、従来なら、前述したように、カルマンフィルタの更新周期を相関時定数より短くすることはできなかったが、この発明ではこの制限を除くために、有色雑音を確率過程でモデル化することにより、カルマンフィルタの理論の前提を満たす表示形式に〔数４〕を変形する。
【００３０】
説明を簡単にするために、ＧＰＳの北方向および東方向の位置誤差（ν_n ，ν_e ）が、分散σ₃ ²、相関時定数τ₂ をもつマルコフ過程（１次マルコフ過程）でモデル化した場合について説明する。
【００３１】
この場合、北方向および東方向の位置誤差ν_n ，ν_e の自己相関関数は
【００３２】
【数５】

【００３３】
で表され、観測雑音νは、ウィナーヒンチンの自己相関関数とパワースペクトルとの関係より、〔数６〕のように、白色雑音ｗで駆動される方程式で表すことができる。
【００３４】
【数６】

【００３５】
次に、〔数４〕を微分し、〔数３〕、〔数４〕、〔数６〕を使って整理すると、〔数７〕が得られる。
【００３６】
【数７】

【００３７】
〔数７〕でＨ＝ＣＡ−ＦＣ，Ｚ＝Ｙ′−ＦＹとおくと、〔数７〕は〔数８〕になる。
【００３８】
【数８】

【００３９】
上記Ｙ′はＩＮＳとＧＰＳの速度差から観測でき、新たにＺを観測値、Ｈを観測行列として、〔数８〕を観測方程式とすれば、〔数８〕は、観測雑音が白色雑音ｗであるので、カルマンフィルタの理論の前提を満たす表示形式に変形した観測方程式になる。ここで相関時定数τ₂ と観測雑音ｗ_n ＝ｗ_e は観測方程式の設計要素であり、設計により定まる。例えば、ＧＰＳ受信機の測定位置の誤差を収集し、誤差の自己相関関数を求めることにより、τ₂ ，ｗ_n ，ｗ_e を決定する。
【００４０】
〔３〕 カルマンフィルタ
〔数３〕のシステム方程式と〔数８〕の観測方程式を連続時間系から更新周期Δの離散時間系に変換すると、システム方程式は〔数９〕となり、観測方程式は〔数１０〕となる。ここで、ｋは０以上の整数であり、ｋ番目の時刻はΔｋで表される。この連続時間系から離散時間系の変換については「有本著；カルマン・フィルタ，産業図書，昭和52年」に示されている。
【００４１】
【数９】

【００４２】
【数１０】

【００４３】
システム方程式と観測方程式からカルマンフィルタの式のパラメータを決定する方法は周知技術であるので、ここではシステム方程式〔数９〕と観測方程式〔数１０〕から導いたカルマンフィルタの式のみを〔数１１〕〜〔数１５〕に示す。
【００４４】
伝播式
【００４５】
【数１１】

【００４６】
このように、推定値を意味する^ （ハット）付き記号を以下２文字で表す。
【００４７】
【数１２】

【００４８】
更新式
【００４９】
【数１３】

【００５０】
【数１４】

【００５１】
【数１５】

【００５２】
以上に述べたように、このカルマンフィルタは、状態変数をランジュバン方程式でモデル化することにより、単純化したシステム方程式と、有色の観測雑音を１次マルコフ過程でモデル化することにより、白色の観測雑音をもつ表示形式に変形した観測方程式とを基にして構成されている。
【００５３】
なお、観測雑音が白色雑音と有色雑音を含む場合は、有色雑音をシステム方程式に組み込むことにより、前述のカルマンフィルタと同様のカルマンフィルタが構成できる。
【００５４】
次に、図１に示したハイブリッド航法装置３の処理手順の一例を、図２を参照して説明する。
先ず、上記システム方程式のＡ_k=0 ，Ｕ_k=0 、観測方程式のＨ_k=0 ，Ｗ_k=0 を設定する。Ａ_k=0 は〔数９〕で示したシステム行列であり、相関時定数τ₁ で定まり、Ｕ_k=0 は同じく〔数９〕で示した加速度誤差ｕの共分散行列にΔを乗じたものであり、これらの値は設計要素である。また、Ｈ_k=0 は〔数１０〕に示した観測行列であり、相関時定数τ₂ で定まり、Ｗ_k=0 は〔数６〕で示した白色雑音ｗの共分散行列に１／Δを乗じたものであり、これらの値も設計要素である。
【００５５】
次に、推定値の初期値Ｘ^ _k=0 と共分散の初期値Ｐ_k=0 を設定する。
【００５６】
その後、更新タイミングとなれば、ＧＰＳ受信機２より北方向の位置Ｘ_N ，東方向の位置Ｘ_E ，北方向の速度Ｖ_N ，東方向の速度Ｖ_E をそれぞれ読み込む。また、ＩＮＳ航法装置１より北方向の位置Ｘ_CN，東方向の位置Ｘ_CE，北方向の速度Ｖ_CN，東方向の速度Ｖ_CEをそれぞれ読み込む。
【００５７】
続いて、
【００５８】
【数１６】

【００５９】
をＺ_k として求める。また、事前推定値Ｘ^ _k/k-1 を〔数１１〕から求める。ここで〔数１１〕のＡ_k は〔数９〕に示したように、ｋに係わらず一定であり、上記設定値Ａ_k=0 を用いる。
【００６０】
続いて、事前共分散Ｐ_k/k-1 を〔数１２〕から求める。ここでＵ_k は〔数９〕に示したように北方向および東方向の加速度誤差ｕ_n ，ｕ_e で決定されるので、ｋに係わらず一定であり、上記設定値Ｕ_k=0 を用いる。
【００６１】
続いて、フィルタゲインＧ_k を〔数１３〕から求める。ここでＨ_k は〔数１０〕に示したようにｋに係わらず一定であり、上記設定値Ｈ_k=0 を用いる。また、Ｗ_k も〔数１０〕に示したように、ｋに係わらず一定であり、上記設定値Ｗ_k=0 を用いる。
【００６２】
さらに、続いて、事後共分散Ｐ_k/k を〔数１５〕から求める。
そして、事後推定値Ｘ_k/k を〔数１４〕から求め、これを出力する。
【００６３】
以上の処理を更新タイミング毎に繰り返すことにより、推定誤差が急速に減少し、図１に示したハイブリッド航法装置３から、誤差分の補正された位置および速度のデータが出力される。
【００６４】
なお、〔数１１〕，〔数１２〕に示した伝播式の更新周期をΔ／Ｎ（Ｎ：２以上の整数）として、カルマンフィルタの更新周期（誤差推定周期）より短時間周期で、ＩＮＳの位置と速度の推定誤差を、その推移を補外することにより求め、ＩＮＳの位置と速度を補正してもよい。
【００６５】
また、以上に述べた例は、２次元の位置と速度を求めるものであったが、同様にして３次元の位置と速度を求めることもできる。
【００６６】
【発明の効果】
この発明によれば、高精度の位置と速度情報が得られるほか、次の利点がある。
(1) システム方程式をランジュバン方程式でモデル化することにより、システム方程式形式が単純となるため、設計要素が少なくなって、カルマンフィルタのチューニングが容易になる。また、システム方程式の形式が単純となるため、カルマンフィルタのチューニング時のシミュレーションと実際の環境とが少し異なっていても、実際の運用時に大きな推定誤差が生じることがなく、いわゆるロバスト性も高まる。
【００６７】
(2) 有色の観測雑音を１次マルコフ過程でモデル化し、観測方程式を白色の観測雑音をもつ表示形式に変形することにより、カルマンフィルタの更新周期は、観測雑音の相関時定数に制限されることなく、任意に設定できる。そのため、カルマンフィルタの更新周期を短くでき、誤差推定の精度が劣化することもない。
【００６８】
(3) 設計要素はプラットフォーム方式のＩＮＳと、ストラップダウン方式のＩＮＳとで異なるが、カルマンフィルタのアルゴリズムは同一となるので、カルマンフィルタの演算処理部分を変えることなく、いずれのタイプの慣性航法装置を用いたハイブリッド航法装置にも適用できる。
【図面の簡単な説明】
【図１】この発明の実施形態に係るハイブリッド航法装置の構成を示す図
【図２】同ハイブリッド航法装置における処理手順を示すフローチャート
【図３】カルマンフィルタのチューニング結果の例を示す図
【符号の説明】
１−ＩＮＳ航法装置
２−ＧＰＳ受信機
３−ハイブリッド航法装置
４−カルマンフィルタ
５−加算器[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a hybrid navigation apparatus that combines radio navigation and inertial navigation.
[0002]
[Prior art]
2. Description of the Related Art Conventionally, as a navigation device for an aircraft or a ship, a so-called hybrid navigation device has been adopted in which a plurality of types of navigation means are combined to compensate for their respective drawbacks, thereby increasing the measurement accuracy of position, speed, and the like. Further, a Kalman filter is used to estimate the current state (measurement error such as position and velocity) from the measured time series data.
[0003]
For example, “Murata, Shingu, Ono, Zhang Rei; GPS navigation flight experiments at the Japan Space Technology Research Institute, Journal of the Japan Aerospace Society, Vol. 41, No. 468, January 1991” (GPS) and inertial navigation system (INS) hybrid navigation has been reported. In such hybrid navigation of GPS and INS, GPS and INS position information or position and speed information is input to the Kalman filter, and the state variable INS position (2 or 3) and speed (2 or 3) are input. ), Acceleration (2 or 3), azimuth (1), posture (2), gyro (3) errors are optimally estimated, and INS position, velocity, azimuth, and posture are optimally estimated and corrected. Thus, it is configured to output highly accurate position, speed, azimuth, and posture information.
[0004]
[Problems to be solved by the invention]
FIG. 3 shows an example of the transition between the actual error and the estimation error for three types of error estimation of the Kalman filter. Here, the vertical axis represents the mean square value of errors, and the horizontal axis represents elapsed time. The accuracy of position and velocity by hybrid navigation depends on the accuracy of error estimation of Kalman filter. In order to obtain the optimum estimated value of the error with the Kalman filter, as shown in FIG. 3A, the estimated error is underestimated to be less than the actual error, or as shown in FIG. Therefore, it is necessary to appropriately tune the Kalman filter so that the estimated error is approximated to the actual error so as not to be in an overestimate state in which the estimated error is estimated to be larger than the actual error. .
[0005]
However, as described above, the number of state variables of the conventional Kalman filter used for hybrid navigation is more than 12, and for example, between the speed error and the azimuth and attitude error, between the azimuth and attitude error and the position error, Since there is an interdependence relationship between the azimuth and the attitude error, the system equation becomes complicated as the system matrix representing the process of generating the measurement error of the inertial navigation system becomes complicated.
[0006]
As described above, in the conventional hybrid navigation apparatus, since the system equation is complicated, it is complicated to tune the Kalman filter so that the Kalman filter can optimally estimate the error.
[0007]
In general, in a radio navigation apparatus such as a GPS receiver, a measurement error becomes colored noise having autocorrelation accompanying signal processing. Therefore, the observation noise included in the observation equation becomes colored noise.
[0008]
However, the theory of Kalman filter assumes that the observation noise is white noise and that it is regular (there is an inverse matrix of the covariance matrix of the observation noise). Therefore, in the past, when the observation noise is colored noise, in order to satisfy the above assumption, the observation noise can be regarded as substantially white noise by setting the update period of the Kalman filter longer than the correlation time constant of the observation noise. ing.
[0009]
However, if the update cycle of the Kalman filter becomes long, the position and speed can be corrected only in the update cycle.
[0010]
In this way, in conventional hybrid navigation systems, as a result of signal processing, observation noise generally becomes colored noise, so the update period of the Kalman filter is limited to the correlation time constant of observation noise, the update period becomes longer, and error estimation The accuracy of the will also deteriorate.
[0011]
An object of the present invention is to solve the problem of complicated Kalman filter tuning due to complicated system equations, and the problem of limiting the Kalman filter update period limitation due to noise coloring due to signal processing in a radio navigation device It is to provide a navigation device.
[0012]
[Means for Solving the Problems]
This invention is a hybrid navigation device that inputs observation values from a radio navigation device and an inertial navigation device, and corrects the observation values of the inertial navigation device by a Kalman filter.
Modeling the measurement error generation process of the inertial navigation system using the Langevin equation, a vector X with the measurement error as a state variable, a system matrix A determined by the correlation time constant, and a system equation with system noise u converted to a discrete time system X _k = A _k-1 X _k-1 + u _k-1 (k is a time step) is a discrete system equation,
Equation Z _k = The measurement error, which is the colored noise of the radio navigation device, is modeled by a Markov process and the observation noise is white noise, and the observation equation by the vector X, the observation matrix H and the observation noise w is converted into a discrete time system. Let H _k X _k + w _k (k is the time step) be a discrete system observation equation,
A Kalman filter composed of the discrete system equation and the discrete system observation equation is used.
[0013]
In this way, tuning is simplified by modeling and simplifying the system equation with the Langevin equation. In addition, the update period of the Kalman filter can be shortened by converting the noise included in the observation equation into white noise.
[0014]
DETAILED DESCRIPTION OF THE INVENTION
FIG. 1 is a block diagram showing a configuration of a hybrid navigation apparatus according to an embodiment of the present invention. Here, INS1 is an inertial navigation device that uses a gyroscope and an accelerometer to determine the speed of the aircraft equipped with them, and the GPS receiver 2 receives radio waves from GPS satellites to determine the position and speed of the aircraft. It is a device to measure. Reference numeral 3 denotes a hybrid navigation device for inputting position and speed information from the INS 1 and the GPS receiver 2, estimating an error of the INS position and speed, and correcting the position and speed of the INS with the estimated error. To output highly accurate position and velocity information.
[0015]
The Kalman filter 4 in the hybrid navigation apparatus 3 optimally estimates the error of the INS position and speed from the information on the respective positions and speeds input from the INS navigation apparatus 1 and the GPS receiver 2.
[0016]
In order to simplify the explanation, a description will be given of a hybrid navigation apparatus that eliminates the conventional drawbacks in the two-dimensional case.
[0017]
[1] System Equation As described above, since the system equation has been complicated conventionally, tuning of the Kalman filter has been complicated. The invention first simplifies tuning by simplifying the system equations.
[0018]
The INS position error is caused by the error of the gyroscope and the accelerometer. More than 70% of them depend on the gyro error, and the effect of the accelerometer error is small. This is shown in “Hayakawa, Hoshino; Inertial element of inertial navigation system, Journal of Japan Aerospace Society, Vol.31, June 1983”. The relationship between the INS position error variance and the gyro error is given by [Equation 1]. This relationship is shown in “Diesel JW; GPS / INSINTEGRATION BY FUNCTIONAL PARTITIONING”.
[0019]
[Expression 1]

[0020]
In [Equation 1], the dispersion Q (t) of the INS position error is such that a term proportional to the time becomes dominant over the vibration term when a time equal to or longer than a quarter of the Schura cycle is 84.4. After one Shura cycle, the term proportional to time is more than five times the vibration term, and the vibration term can be ignored. In other words, after one Shura cycle, the variance of the INS position error increases substantially in proportion to time.
[0021]
This indicates that the INS position error can be modeled by the Langevin equation.
[0022]
When a vector having an INS error as a state variable is expressed by [Equation 2] and modeled by the Langevin equation, A is a system matrix, u is a system noise, and the system equation is expressed by [Equation 3]. However, T shows transposition.
[0023]
[Expression 2]

[0024]
[Equation 3]

[0025]
Thus, by modeling the INS position error using the Langevin equation, the system equation could be greatly simplified. The correlation time constant τ ₁ and the north and east acceleration errors u _n = u _e are design elements of the system equation and are determined by the system design. In addition, as indicated by X ′ in [Equation 3], a symbol with a “(dot)” indicating differentiation is represented by two characters below.
[0026]
[2] Observation Equation When the observation vector is (INS position) − (GPS position), the observation equation can be expressed by [Equation 4].
[0027]
[Expression 4]

[0028]
As described above, the positioning error of the GPS receiver is caused by the Markovian ionospheric delay included in the received signal, or the process of updating the measurement position by tracking the phase of the pseudo-noise code included in the received signal. Therefore, it becomes colored noise having autocorrelation, and thus the observation noise included in the observation equation becomes colored noise.
[0029]
In the case where the observation noise is colored noise, the Kalman filter update period could not be made shorter than the correlation time constant as described above. (4) is transformed into a display format that satisfies the premise of the Kalman filter theory.
[0030]
To simplify the explanation, GPS position errors (ν _n , ν _e ) in the north and east directions are modeled by a Markov process (first order Markov process) with variance σ ₃ ² and correlation time constant τ ₂ . The case will be described.
[0031]
In this case, the autocorrelation function of the position errors ν _n and ν _{e in} the north and east directions is
[Equation 5]

[0033]
The observation noise ν can be expressed by an equation driven by the white noise w as shown in [Equation 6] from the relationship between the Wiener Hinchin autocorrelation function and the power spectrum.
[0034]
[Formula 6]

[0035]
Next, [Equation 4] is differentiated and rearranged using [Equation 3], [Equation 4], and [Equation 6] to obtain [Equation 7].
[0036]
[Expression 7]

[0037]
If [Expression 7] and H = CA-FC and Z = Y'-FY, then [Expression 7] becomes [Expression 8].
[0038]
[Equation 8]

[0039]
Y ′ can be observed from the difference in velocity between INS and GPS. If Z is an observation value, H is an observation matrix, and [Equation 8] is an observation equation, then [Equation 8] Therefore, the observation equation is transformed into a display format that satisfies the premise of the Kalman filter theory. Here the correlation time constant tau ₂ and the observation noise w _n = w _e is a design element of the observation equation determined by the design. For example, to collect the error of measurement positions of the GPS receiver, by obtaining the autocorrelation function of the error, to determine _{τ 2, w n, w e} .
[0040]
[3] When the system equation of the Kalman filter [Equation 3] and the observation equation of [Equation 8] are converted from the continuous time system to the discrete time system of the update period Δ, the system equation becomes [Equation 9], and the observation equation becomes [Equation 10]. ]. Here, k is an integer greater than or equal to 0, and the k-th time is represented by Δk. This conversion from continuous-time system to discrete-time system is described in “Arimoto, Kalman Filter, Sangyo Tosho, 1977”.
[0041]
[Equation 9]

[0042]
[Expression 10]

[0043]
Since the method for determining the parameters of the Kalman filter equation from the system equation and the observation equation is a well-known technique, only the Kalman filter equation derived from the system equation [Equation 9] and the observation equation [Equation 10] is used here. It is shown in [Equation 15].
[0044]
Propagation type 【0045】
## EQU11 ##

[0046]
In this way, the symbol with ^ (hat) that means the estimated value is represented by the following two characters.
[0047]
[Expression 12]

[0048]
Renewal formula [0049]
[Formula 13]

[0050]
[Expression 14]

[0051]
[Expression 15]

[0052]
As described above, this Kalman filter is designed to model the state variables with the Langevin equation, to model the simplified system equations and the colored observation noise with a first-order Markov process, thereby to produce white observation noise. It is constructed based on the observation equation transformed into a display format with.
[0053]
When the observation noise includes white noise and colored noise, a Kalman filter similar to the aforementioned Kalman filter can be configured by incorporating the colored noise into the system equation.
[0054]
Next, an example of the processing procedure of the hybrid navigation apparatus 3 shown in FIG. 1 will be described with reference to FIG.
First, A _{k = 0} and U _{k = 0 in} the system equation and H _{k = 0} and W _{k = 0} in the observation equation are set. A _{k = 0} is a system matrix expressed by [Equation 9], which is determined by the correlation time constant τ ₁ , and U _{k = 0} is obtained by multiplying the covariance matrix of the acceleration error u also expressed by [Equation 9] by Δ. These values are design elements. H _{k = 0} is the observation matrix shown in [Equation 10], which is determined by the correlation time constant τ ₂ , and W _{k = 0} is 1 / Δ in the covariance matrix of the white noise w shown in [Equation 6]. These values are also design elements.
[0055]
Next, an initial value P _{k = 0} of the covariance between the initial value X ^ _{k = 0} estimates.
[0056]
Thereafter, at the update timing, the position X _{N in} the north direction, the position X _{E in} the east direction, the speed V _{N in} the north direction, and the speed V _{E in the} east direction are read from the GPS receiver 2. Further, the north position X _CN , the east position X _CE , the north direction velocity V _CN , and the east direction velocity V _CE are read from the INS navigation device 1.
[0057]
continue,
[0058]
[Expression 16]

[0059]
_Is determined as Z _k . Also, a prior estimated value X ^ _{k / k-1} is obtained from [Equation 11]. Here, A _k in [ _Equation 11] is constant regardless of k as shown in [Equation 9], and the set value A _{k = 0} is used.
[0060]
Subsequently, the prior covariance P _{k / k−1} is obtained from [Equation 12]. Here, U _k is determined by the acceleration errors u _n and u _e in the north direction and the east direction as shown in [Equation 9], so it is constant regardless of k, and the set value U _{k = 0} is used. .
[0061]
Subsequently, the filter gain G _k is _obtained from [Equation 13]. Here, H _k is constant regardless of k as shown in [Equation 10], and the set value H _{k = 0} is used. Also, as shown in [Equation 10], W _k is constant regardless of k, and the set value W _{k = 0} is used.
[0062]
Subsequently, the posterior covariance P _{k / k} is _obtained from [Equation 15].
Then, a posteriori estimated value X _{k / k} is obtained from [Equation 14] and output.
[0063]
By repeating the above processing at each update timing, the estimation error is rapidly reduced, and corrected position and speed data for the error is output from the hybrid navigation apparatus 3 shown in FIG.
[0064]
Note that the update cycle of the propagation equation shown in [Equation 11] and [Equation 12] is Δ / N (N: an integer of 2 or more), and the INS is updated in a shorter cycle than the update cycle (error estimation cycle) of the Kalman filter. The estimation error of the position and speed may be obtained by extrapolating the transition, and the position and speed of the INS may be corrected.
[0065]
In the example described above, the two-dimensional position and velocity are obtained. However, the three-dimensional position and velocity can be obtained in the same manner.
[0066]
【The invention's effect】
According to the present invention, highly accurate position and velocity information can be obtained, and the following advantages can be obtained.
(1) By modeling the system equation with the Langevin equation, the system equation form becomes simple, so design elements are reduced and the Kalman filter can be tuned easily. Also, since the system equation format is simple, even if the simulation at the time of tuning the Kalman filter is slightly different from the actual environment, a large estimation error does not occur during actual operation, and so-called robustness is enhanced.
[0067]
(2) By modeling colored observation noise by a first-order Markov process and transforming the observation equation into a display format with white observation noise, the update period of the Kalman filter is limited to the correlation time constant of the observation noise. It can be set arbitrarily. Therefore, the update period of the Kalman filter can be shortened, and the accuracy of error estimation does not deteriorate.
[0068]
(3) Although the design elements differ between the platform type INS and the strap-down type INS, the algorithm of the Kalman filter is the same, so any type of inertial navigation system can be used without changing the arithmetic processing part of the Kalman filter. It can also be applied to conventional hybrid navigation systems.
[Brief description of the drawings]
FIG. 1 is a diagram showing a configuration of a hybrid navigation device according to an embodiment of the present invention. FIG. 2 is a flowchart showing a processing procedure in the hybrid navigation device. FIG. 3 is a diagram showing an example of tuning results of a Kalman filter. ]
1-INS navigation device 2-GPS receiver 3-hybrid navigation device 4-Kalman filter 5-adder

Claims (2)

In hybrid navigation that estimates and corrects the measurement error of the inertial navigation device from the measured values of the radio navigation device and the inertial navigation device using the Kalman filter, the measurement error is modeled by the Langevin equation. Is a state vector X, a system matrix A determined by a correlation time constant, and an equation X _k = A _k-1 X _k-1 + u _k-1 (k is time) Step) is a discrete system equation,
Equation Z _k = The measurement error, which is the colored noise of the radio navigation device, is modeled by a Markov process and the observation noise is white noise, and the observation equation by the vector X, the observation matrix H and the observation noise w is converted into a discrete time system. Let H _k X _k + w _k (k is the time step) be a discrete system observation equation,
Hybrid navigation using a Kalman filter composed of the discrete system equation and the discrete observation equation. In the hybrid navigation device that inputs the measured values of the radio navigation device and the inertial navigation device, estimates the measurement error of the inertial navigation device with the Kalman filter, and corrects it,
Modeling the measurement error generation process of the inertial navigation system using the Langevin equation, a vector X with the measurement error as a state variable, a system matrix A determined by the correlation time constant, and a system equation with system noise u converted to a discrete time system X _k = A _k-1 X _k-1 + u _k-1 (k is a time step) is a discrete system equation,
Equation Z _k = The measurement error, which is the colored noise of the radio navigation device, is modeled by a Markov process and the observation noise is white noise, and the observation equation by the vector X, the observation matrix H and the observation noise w is converted into a discrete time system. Let H _k X _k + w _k (k is the time step) be a discrete system observation equation,
The hybrid navigation apparatus which comprised the said Kalman filter from the said discrete system equation and the said discrete system observation equation.

Images