KR102172145B1 - Tightly-coupled localization method and apparatus in dead-reckoning system - Google Patents

Abstract

A method and apparatus for tightly coupled positioning in a guessed navigation system are provided. Based on the first observation data from the inertial sensor, the second observation data from the satellite navigation apparatus, and the third observation data from the Doppler receiving apparatus, a state variable for positioning is set, and tightly coupled positioning is performed based on this.

Description

Tightly-coupled localization method and apparatus in dead-reckoning system

The present invention relates to a tightly coupled positioning method and an apparatus therefor in a guessed navigation system.

With the recent widespread spread of smartphones and the development of positioning technology, positioning technology is the most fundamentally secured technology to provide location-based services (LBS), which is emerging as the most promising mobile core service as a killer application. to be.

The positioning system is largely classified into a relative positioning system and an absolute positioning system.

Relative positioning system (also known as dead reckoning system) is a system that tracks the relative amount of change based on the initial position and direction in tracking the position of an object. It is contrasted with a tracking absolute positioning system. A representative relative positioning system is Pedestrian Dead Reckoning (PDR). If a pedestrian can detect a change in the number of steps and direction of his or her steps when moving a certain path, the pedestrian can track his or her approximate position relative to the starting point based on this information and an estimate of his average stride length. . This is the basic principle of guessed pedestrian navigation.

However, in the case of the pedestrian guess navigation, the estimated value of the pedestrian's position becomes increasingly inaccurate as time progresses. This is a method in which the estimated pedestrian navigation connects the estimate of the relative displacement of the pedestrian corresponding to the number of footsteps in a chain.The relative displacement estimate of each step only has an error that occurs because the actual stride length of the pedestrian is not constant. Rather, it is because an error that intervenes in the estimate of the amount of change in the moving direction is also included. Since these errors are continuously accumulated and reflected in the position estimation result, the position estimation result becomes more and more inaccurate as the pedestrian proceeds forward. This accumulation of errors is called drift, and it is a characteristic that is common to all relative positioning systems.

On the other hand, as an absolute positioning system, a representative global navigation satellite system (GNSS) is exemplified, and in addition, a mobile communication network, Wi-Fi, ultra wideband (UWB), and a method using Bluetooth are being studied.

The satellite navigation system is a system that finds out the position, altitude, and speed of a target on the ground using network information of an orbiting satellite. It is used for military purposes such as missile guidance or for navigation devices such as aircraft, ships, and automobiles. Among them, a representative system is a global positioning system (GPS).

Another absolute positioning system is the Doppler Radar. Doppler radar is a special radar that uses the Doppler effect to generate speed data of distant objects. After measuring the reflected wave from a microwave signal by matching it to a target, the frequency of the returned signal is determined by the movement of the object. Analyze. As a system using Doppler radar, there is a speed gun used to detect speeding vehicles.

The phenomenon in which the observed frequency changes due to the wave source from which the wave is generated and the observer's relative motion is called the Doppler Effect. This phenomenon appears for all waves such as light waves (light) as well as sound waves. The speed of the terminal can be detected using a Doppler radar. When the transmitter and the receiver relatively move within the channel, the receiver receives a signal with a frequency slightly different from the frequency of the signal transmitted by the transmitter, and the speed of the terminal can be estimated based on the frequency change of the received signal. . A device capable of detecting the speed of a terminal using the Doppler effect is called a Doppler receiving device.

및 (광속으로 normalize한) 관측자(단말)의 속도의 곱에 비례한다. 이러한 원리에 기반하여 단말의 속도를 감지해 내는 장치가 도플러 수신 장치이다. Wi-Fi 신호는 보행자 속도에 대응하는 도플러 쉬프트 량의 측정에 사용할 수 있는 주파수 대역에 속해 있다고 알려져 있다.In indoor wireless communication environment, the influence of Doppler shift is carrier frequency.

And the product of the speed of the observer (terminal) (normalized by the speed of light). A device that detects the speed of a terminal based on this principle is a Doppler receiving device. It is known that the Wi-Fi signal belongs to a frequency band that can be used to measure the amount of Doppler shift corresponding to the pedestrian speed.

When a Doppler receiver is used for indoor positioning, it does not require a separate massive infrastructure investment such as other indoor positioning means, and signals are generated even in environments dominated by the multipath effect, which is the biggest obstacle to indoor positioning. The advantage is that it can be applied without knowing the location of the source.

The speculative navigation system boasts high precision in short-range tracking, but has a disadvantage in that precision is significantly degraded due to a drift problem in long-distance tracking. Therefore, there is a need for a method to overcome the drift phenomenon, which is an accumulation phenomenon of relative errors, and to improve the accuracy of the positioning results.

The technical problem to be solved by the present invention is to overcome the accumulation of errors common in relative positioning systems such as inertial sensors and improve the accuracy of positioning results, based on the observation data of the absolute positioning system. It is to provide a tightly coupled positioning method and apparatus for correcting/calibrating a positioning result by a positioning system.

In the tightly coupled positioning method according to a feature of the present invention, based on the first observation data from the inertial sensor, the second observation data from the satellite navigation device, and the third observation data from the Doppler receiver, a state variable for positioning is determined. Setting up; Performing a prediction process of calculating a state predicted value and an error covariance predicted value for the first to third observed data for a current state based on the set state variable; And performing a calibration process of obtaining a new estimated value for positioning by compensating the state prediction value and the error covariance prediction value obtained in the prediction process by a difference between the measured value and the predicted value.

The first observation data may be azimuth angle information. The second observation data includes pedestrian location information, and the location information may be composed of x coordinates and y coordinates. The third observation data may include a stride length.

The third observation data includes the instantaneous speed for each step, the instantaneous speed for each step is calculated based on the stride length of the kth step and the required time △ of the kth step, and the required time △ of the kth step is the time k from the time of departure. It can be derived by dividing the total required time to by the number of steps detected by the accelerometer.

The performing of the prediction process may include predicting a current state based on the state variable to obtain a state predicted value; Predicting an error covariance corresponding to a current state to obtain a predicted error covariance value; And obtaining a predicted measurement value that is an estimated value for the observed data based on the state predicted value.

The performing of the calibration process may include calculating a gain based on the predicted error covariance and actual observed noise; Correcting a state predicted value based on the calculated gain and a difference between an actual measured value and a predicted measured value to obtain a new estimated value; And obtaining a corrected error covariance by correcting the predicted error covariance value based on the calculated gain.

The actual measured value may be first observation data from the inertial sensor, second observation data from a satellite navigation device, and third observation data from a Doppler receiver.

In the setting of the state variable, the state variable may be set based on a new estimated value of a previous step obtained by performing the prediction process and the calibration process based on the first to third observation data.

Meanwhile, the prediction process and the correction process may be performed based on the Kalman filter.

A tightly coupled positioning device according to another aspect of the present invention includes: a first input unit receiving first observation data from an inertial sensor; A second input unit receiving second observation data from a satellite navigation device; A third input unit receiving third observation data from the Doppler receiving device; And performing a prediction process of setting a state variable for positioning based on the first to third observation data, calculating a predicted value for a current state and a predicted error covariance value based on the set state variable, and obtained in the prediction process. And a tightly coupled positioning processor configured to obtain data for positioning by performing a calibration process of obtaining a new estimate value for positioning by compensating the predicted state value and the predicted error covariance value with a difference between the measured value and the predicted value.

The first observation data is azimuth angle information, the second observation data includes pedestrian location information, the location information is composed of x coordinates and y coordinates, and the third observation data may include a stride length. The third observation data includes the instantaneous speed for each step, the instantaneous speed for each step is calculated based on the stride length of the kth step and the required time △ of the kth step, and the required time △ of the kth step is the time k from the time of departure. It can be derived by dividing the total required time to by the number of steps detected by the accelerometer.

The tightly coupled positioning processing unit may include a variable setting unit configured to set the state variable based on a new state estimation value of a previous step acquired according to the prediction process and the calibration process based on the first to third observation data; A first prediction processing unit that predicts a current state based on the state variable to obtain a state predicted value; A second prediction processing unit that predicts an error covariance corresponding to a current state and obtains a predicted error covariance value; And an estimate value calculator that obtains a predicted measurement value that is an estimate value for observation data based on the state predicted value.

The tightly coupled positioning processing unit may include a gain calculation unit that calculates a gain based on the predicted error covariance and actual observed noise; An estimated value correction unit for obtaining a new estimated value by correcting a state prediction value based on the calculated gain and a difference between an actual measured value and a predicted measured value; It may further include an error calculator for obtaining a corrected error covariance by correcting the predicted error covariance value based on the calculated gain.

According to an embodiment of the present invention, it is possible to provide an accurate positioning result by solving an error accumulation problem occurring in a relative positioning system using an inertial sensor or the like, and to stably guarantee the accuracy of the positioning result.

1 is an exemplary diagram showing the Doppler effect when a wave source and an observer are in relative motion.
2 is an exemplary diagram showing the influence of the Doppler effect in an indoor wireless environment.
3 is a conceptual diagram showing the overall operation of the Kalman filter algorithm.
4 and 5 are diagrams showing the structure of a tight coupling positioning device according to an embodiment of the present invention.
6 is a flowchart of a tight coupling positioning method according to an embodiment of the present invention.

Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings so that those of ordinary skill in the art may easily implement the present invention. However, the present invention may be implemented in various forms and is not limited to the embodiments described herein.

In the drawings, parts irrelevant to the description are omitted in order to clearly describe the present invention, and similar reference numerals are assigned to similar parts throughout the specification.

Throughout the specification and claims, when a certain part'includes' a certain element, it means that other elements may be further included rather than excluding other elements unless otherwise stated.

Hereinafter, a method and an apparatus for a tightly coupled positioning in a guessed navigation system according to an embodiment of the present invention will be described.

The positioning system is roughly divided into a relative positioning system and an absolute positioning system.In order to improve the accuracy of the positioning results, for the positioning results by the relative positioning system, correction/calibration of periodic positioning results based on observation data by the absolute positioning system ( Calibration) work is required.

As an absolute positioning system, there is a Global Navigation Satellite System (GNSS), which is typically mounted on a navigation system in an outdoor area. The satellite navigation system is a device that finds the position, altitude, and speed of a target on the ground using network information of an artificial satellite orbiting the earth, and a representative system is a global positioning system (GPS).

In GPS, radio waves (microwaves) transmitted from all 24 satellites are received by the receiver's receiver and the receiver's location is determined. Twenty-four GPS satellites orbit the Earth twice a day and transmit location values to millions of GPS terminals on Earth, for example every second. The artificial satellites are arranged at regular intervals in 6 rotational orbits of about 20,000 km. This is because the Earth is not a flat shape but a round three-dimensional shape, so four GPS satellites are required to find a location.

Originally started as a military project, GPS is widely used as a useful tool for tracking and monitoring in commercial services or scientific research, and the accurate time of GPS enables time synchronization and handoff switching, thereby enabling financial, mobile network operation, and even power grid control. It is also useful for everyday activities such as.

The receiver of such a satellite navigation system checks the position of the GNSS satellite and the clock of the GNSS satellite through the navigation message received from the satellite, detects the code sent on the carrier wave, calculates the time taken by the satellite signal to the receiver, and multiplies the speed of the radio wave. Find the distance between the GNSS satellite and the receiver. However, due to the radio wave environment, the error of the clock built into the GNSS receiver, and the internal error of the receiver, the calculated distance is not an actual distance but a pseudorange.

When the coordinates of the GNSS receiver are [x, y, z], the time deflection is b, and the coordinates of the i-th GNSS satellite are [xi, yi, zi], the pseudo distance between the i-th satellite and the receiver can be expressed by the following equation: have.

Where c represents the speed of propagation (speed of light).

In order to obtain the coordinates of the GNSS receiver, four equations are required because [x, y, z, b] four unknowns must be known. Therefore, it is necessary to receive satellite signals from at least four GNSS satellites.

Another absolute positioning system is the Doppler Radar. Doppler radar is a special radar that uses the Doppler effect to generate speed data of distant objects. Doppler radar measures the reflected wave from the 　 microwave 　 signal to the target, and then analyzes how much the frequency of the returned signal is changed by the movement of the object.

The principle of detecting the speed of the terminal by the Doppler radar can be explained through the Doppler effect as follows.

When the transmitter and the receiver move relatively in the channel, the receiver experiences a phenomenon of receiving a signal with a frequency slightly different from the frequency of the signal transmitted by the transmitter. A daily life experience that stems from the same principle is the phenomenon that the sound is high when the car approaches while sounding a siren, and the sound is low when the car passes in front of the observer and moves away. The principle that this phenomenon occurs can be explained as follows.

In general, when the wave source and the observer are in relative motion, when they are close to each other, it is heard at a higher frequency (higher tone) than the actual frequency of the wave source, and when they are distant from each other, it is heard at a smaller frequency (lower tone) than the actual frequency. In this way, the phenomenon in which the observed frequency varies due to the relative motion of the wave source and the observer is called the Doppler Effect. This phenomenon appears not only for sound waves but also for all waves such as light waves (light).

When the wave source and the observer are in relative motion, the speed of the sound is V, the speed of the sound source is v, the frequency of the sound source is f ₀ , the frequency of the sound that the observer hears is f, and the observer O is from the sound source S to the speed u. Think about the case of getting away.

1 is an exemplary diagram showing the Doppler effect when a wave source and an observer are in relative motion.

If the direction of the observer at the wave source is (+), the frequency f that the observer hears in the above two equations can be expressed as follows.

Here, when ｕ and ｖ are opposite to the +V direction, insert a (-) sign and substitute it in the upper equation to use.

The influence of the Doppler effect will be examined in an indoor wireless communication environment.

2 is an exemplary diagram showing the influence of the Doppler effect in an indoor wireless environment.

, 파원(Source)의 속도를

, 관측자(Observer)의 속도를

라고 하면, 위의 수학식 2에 따라 수신된 파동의 주파수는 다음과 같이 나타낼 수 있다. 2, in the case of a general wave such as an electromagnetic wave caused by Wi-Fi, the speed of the wave is

, The speed of the source

, Speed of the observer

If so, the frequency of the wave received according to Equation 2 above can be expressed as follows.

이고

이므로, 다음과 같이 식이 성립된다. In the case of electromagnetic waves, Equation 3 above is applied as it is, in this case

ego

Therefore, the equation is established as follows.

는 다음과 같이 나타낼 수 있다. Therefore, the amount of Doppler shift

Can be expressed as

및 관측자(단말)의 속도((광속으로 노말라이즈(normalize)한 속도임)의 곱에 비례한다는 사실을 알 수 있다. 이러한 원리에 기반하여 단말의 속도를 감지해 내는 장치가 도플러 수신 장치이다. From Equation 5 above, the influence of Doppler shift in the indoor wireless communication environment is the carrier frequency

And it can be seen that it is proportional to the product of the speed of the observer (terminal) (which is the speed normalized to the speed of light) A Doppler receiving device is a device that detects the speed of the terminal based on this principle.

According to Equation 5 for the above Doppler shift amount, it can be seen that the Doppler shift amount is dependent on the carrier frequency.

Inertial navigation systems and Doppler radars have been mainly applied as navigation systems for aircraft and submarines since the 1950s. As described above, the speculative navigation system boasts high precision in short-range tracking, but in long-distance tracking, the precision is significantly degraded due to a drift problem. On the other hand, the Doppler radar is relatively inferior in precision but does not have a drift problem during long-distance tracking, so it is possible to overcome the drift problem in long-distance tracking and provide a high-precision positioning solution by using both positioning systems in combination. do.

In order to provide such a high-performance positioning solution, a method in which completely different types of positioning systems are complementarily combined and effectively used is commonly referred to as tightly-coupled positioning. Traditionally, the tool used to implement such tightly coupled positioning solutions is the Kalman Filter or its variant, the Extended Kalman Filter.

Hereinafter, a Kalman filter will be described.

The Kalman Filter is a recursive filter that tracks the state of a linear system from observational data containing noise, and was developed by Rudolf E. Kalman in the early 1960s. It was applied to the problem of orbit estimation of the Apollo spacecraft, and later it was widely used in engineering fields such as computer vision, robots, radar, rockets, satellites, missiles, and controls, as well as stocks, econometrics, weather forecasting, and population forecasting. It is also used in the social science field. The Kalman filter is an algorithm used to estimate new results by removing the noise contained in the data using old and new measurement data, and operates recursively on a linear system. This is because the measurement data we get from a certain object contains uncertainty information (Noise), which can be understood as the operation of filtering out the impurities as a filter of a water purifier. That is, it is an algorithm that selects a desired signal or information from measurement data or signals including noise (having a Gaussian distribution), and is a prediction system based on probability.

The movement of the natural world is governed by physical laws, so it can be predicted to some extent. The demand for estimating the current state with accumulated past data and the best data currently available occurs everywhere in natural or social phenomena. From a point of view, missile tracking or stock flows have very similar properties.

In practical applications, since the system is nonlinear and the noise is not Gaussian in many cases, a modified form of the Kalman filter is required, and the Extended Kalman Filter is most often used. The extended Kalman filter is similar to the linearized Kalman filter, but has the characteristic that the linearized reference point is continuously updated.

Signals of the natural world take various forms such as video, audio, coordinates, pressure, etc. In most cases, physical noise by the sensor itself is added while passing through the sensor, and is usually removed by a filter.

The Kalman filter is a tool used to predict key variables (aircraft attitude, moving object position, tracking object position, etc.) that are required during operation of each system in various fields such as vehicle position control, navigation and image processing. to be. In order for the Kalman filter to remove noise from the extracted signal, the process to be modeled must be described as a linear system.

Linear systems are generally expressed by two formulas:

The process/system model is expressed by Equation 6 below.

는 상태 천이(state transition) 행렬을 나타내며, V_k는 시간 k에서의 프로세스 노이즈(process noise)를 나타낸다.Here, X _k represents the state variable of the Kalman filter at time k,

Denotes a state transition matrix, and V _k denotes process noise at time k.

The observation/sensor model can be expressed as follows.

는 관측 행렬을 나타내며, W_k는 시각 t에서의 관측/센서 노이즈(measurement/sensor noise)를 나타낸다. Z_k는 센서 등을 이용해서 측정된 값을 나타낸다.here,

Denotes the observation matrix, and W _k denotes the observation/sensor noise at time t. Z _k represents a value measured using a sensor or the like.

The process/system model describes the overall behavior of the system, also known as the equation of state. Observation/sensor models represent measurable values among system signals, and are also called output equations. X _k is a vector representing the state of the system, and Z _k is a value measured using a sensor or the like. In addition, V _k and W _k are noise (error), in particular V _k is process noise, and W _k is observation/sensor noise. This noise is the most important in the Kalman filter.

First of all, the process state vector X _k represents all information about the current state of the system (ex. location information of the vehicle, angle, etc.). The important thing is that this state variable cannot be measured directly. Instead, a certain amount of value by the observation / sensor noise W _k is measured disheveled Z _k. Using this measured Z _k , you can calculate X _k inversely. However, the observed value Z _k cannot be used as it is because the value is changed by the observation/sensor noise W _k . Since all sensors basically contain errors, the Kalman filter is what is needed as a tool to accurately predict the state X _k of the system.

The overall operation of the Kalman filter can be summarized as extracting an estimate from the measured value.

Based on the system model, the state and error covariance are predicted in the next step (prediction process), and a new estimate value is calculated by compensating for the difference between the obtained predicted value and the measured value (correction process).

As shown in FIG. 3, the Kalman filter algorithm is configured such that the above two steps are cyclically engaged.

3 is a conceptual diagram showing the overall operation of the Kalman filter algorithm.

이전 상태, K는 Kalman 이득(Gain)이고, P는 추정오차 공분산 행렬, ^는 상태변수를 예측한다는 것을 나타낸다. 그리고, X^θ는 현 상태에 대한 교정 직전의 예측값으로서, ^θ는 이전 상태(정확히는, 아래 첨자 _k+1|k와 동일한 의미)를 의미하며,

의

는 현 상태에 대한 교정 직후의 예측값(정확히는, 아래 첨자 _k+1|k+1과 동일한 의미)을 의미한다. 그리고, Q는 예측 노이즈 공분산, R은 관측 노이즈 공분산을 나타낸다. In the attached Figure 3,

In the previous state, K is the Kalman gain, P is the estimated error covariance matrix, and ^ indicates that the state variable is predicted. In addition, X ^θ is a predicted value immediately before calibration for the current state, and ^θ refers to the previous state (exactly, the same meaning as the subscript _k+1|k ),

of

Denotes the predicted value immediately after correction for the current state (exactly, it means the same as the subscript _k+1|k+1 ). In addition, Q represents the predicted noise covariance, and R represents the observed noise covariance.

Initially, an initial prediction of X and P should be provided.

(1) Prediction step (time update): Prediction based on previous data

에 F를 곱하고 시스템입력 U_k를 더하여 구한 x^θ는 순수한 상태 예측값이다.1) Previous state vector

X ^θ obtained by multiplying by F and adding the system input U _k is the pure state predicted value.

2) This is a step to perform error correction, and it determines how much correction will be made based on the collected data (process noise prediction).

(2) Calibration step (observation update): calibration with new measured values

1) Find the Kalman Gain.

(Calculated using process noise (prediction error) and observed noise (actual error))

를 구한다.2) Present state vector by adding observation/sensor noise W _k

Find

3) Calculate the estimated error covariance P, which is a prediction error.

In an embodiment of the present invention based on such a Kalman filter, a tight coupling is positioned.

In an embodiment of the present invention, a tight coupling positioning is performed to periodically correct/calibrate a positioning result by a relative positioning system based on observation data by an absolute positioning system.

In order to overcome the accumulation of errors common in relative positioning systems using inertial sensors, etc., and to improve the accuracy of positioning results, the above results are corrected by the relative positioning system based on the observed data from the absolute positioning system, and the relative positioning system As a means to calibrate by removing the influence of the error characteristic of, tightly coupled positioning is performed using the position measurement value by the satellite navigation system and the velocity measurement value by the Doppler receiver.

An inertial sensor such as a gyroscope is a relative positioning means that measures movement information relative to a reference point and has an error accumulation characteristic over time, whereas a satellite navigation system and a Doppler receiver both observe absolute position/speed. As an absolute positioning means, positioning is performed by taking only the strengths of both positioning means and complementarily closely coupling each other. The satellite navigation system mainly functions as a positioning means for calibration, which is useful outdoors, and the Doppler receiver can function as a positioning means for indoor calibration because the satellite navigation signal is not accessible indoors.

The positioning device, that is, the tightly coupled positioning system according to an embodiment of the present invention builds a system model based on the assumption that the pedestrian's stride is constant. In the measurement and update step, the Kalman filter approximates the estimate of the velocity component to the actual observed value, so that a system model can be constructed based on the above assumption.

The tightly coupled positioning system can receive observation data from various positioning resources. In particular, the observation data from the inertial sensor, the Doppler receiver, and the GPS receiver of the satellite navigation system are provided. Perform.

Various sensors, such as a gyroscope and an accelerometer, may be included as the inertial sensor. Here, a gyroscope is used as an example, but the present invention is not limited thereto.

The inertial sensor functions as a relative positioning means that measures and provides relative movement information of a terminal using a specific position as a reference point. Observation data provided from the inertial sensor is referred to as first observation data for convenience of explanation.

On the other hand, the GPS receiver used in the satellite navigation system and the Doppler receiver using the Doppler effect are both absolute positioning means for observing the absolute position/speed, and the GPS receiver functions as a calibration means outdoors, and the Doppler receiver Functions as an indoor calibration tool. For convenience of explanation, observation data provided from the GPS receiver is referred to as second observation data, and observation data provided from the Doppler receiver is referred to as third observation data.

The first observation data includes azimuth angle information, and indicates an orientation of a pedestrian traveling direction. The second observation data includes pedestrian location information, and the location information consists of x-coordinates and y-coordinates. The third observation data includes a stride length and an instantaneous speed for each step. The instantaneous speed for each step is calculated based on the stride length of the kth step and the required time Δ of the kth step. Here, the required time △ of the k-th step can be derived by dividing the total required time from the start time to the time k by the number of steps detected by the accelerometer.

Closely coupled positioning is performed based on the first to third observation data. To this end, a Kalman filter is used in an embodiment of the present invention. However, the present invention is not limited thereto, and for example, an extended Kalman filter may be used.

In an embodiment of the present invention based on the Kalman filter, in order to extract an estimated value from a measured value, a prediction process and a correction process are performed, and a prediction process and a correction process are repeatedly performed.

In the prediction process, based on the system model, the predicted state and the error covariance when a predetermined input is applied are calculated based on the state estimated in the previous step. In the calibration process, a new estimated value is calculated by compensating for the difference between the predicted value and the measured value obtained in the prediction process.

First, at time k, the state variable X _k of the Kalman filter is set as follows.

Here, x _k represents the x coordinate of the second observation data representing the pedestrian position, and y _k represents the y coordinate of the second observation data representing the pedestrian position. s _k denotes the pedestrian's stride corresponding to the third observation data, and Ψ _k denotes the azimuth (Orientation) of the pedestrian traveling direction corresponding to the first observation data. The azimuth angle represents the angle measured counterclockwise from the positive x-axis. This azimuth angle may be detected and provided through a gyroscope or a geomagnetic sensor.

The estimated value, that is, the state variable X _k , is set as above based on the measured value, that is, the first to third observed data, and then the next state and the error covariance are predicted based on the system model.

Based on Equation 6 above, the next state can be predicted as follows.

는 이전 상태 벡터를 나타내며,

는 상태 변수를 예측하는 것을 나타낸다. x^θ는 현재 상태에 대한 예측값으로, 순수한 상태 예측값 즉, 본 발명의 실시 예에 따른 교정 과정이 수행되기 전의 예측값을 나타낸다. ^θ는 이전 상태( k+1|k와 동일한 상태)를 나타낸다. here

Represents the previous state vector,

Denotes predicting the state variable. x ^θ is a predicted value for the current state, and represents a pure state predicted value, that is, a predicted value before the calibration process according to an embodiment of the present invention is performed. ^θ represents the previous state (the same state as k+1|k).

와 같이 나타낼 수 있다. 이러한 상태 변수의 천이를 수식으로 표현하면 다음과 같다. Based on Equation 9 related to this system model, if the system input U _k = 0, the next state variable

It can be expressed as The transition of the state variable is expressed as an equation as follows.

And if Equation 10, which is the state transition relation, is expressed as a determinant, it is as follows.

는 상태 천이(state transition) 행렬을 나타낸다. here,

Denotes a state transition matrix.

Meanwhile, the predicted error covariance can be expressed as follows.

은 현재 상태에서 예측된 오차 공분산을 나타내며, Q는 예측 노이즈 공분산을 나타낸다.

Represents the predicted error covariance in the current state, and Q represents the predicted noise covariance.

And the current state predicted value x ^θ and the measured value are made in the following relationship.

는 현재 상태 예측값을 토대로 예측된 측정값을 나타내며, 예측 측정값 즉, 추정값이라고 명명할 수 있다. here,

Denotes a predicted measurement value based on the current state predicted value, and may be referred to as a predicted measurement value, that is, an estimated value.

On the other hand, based on the observation model according to Equation 7, the relationship between the state variable X _k and the measured value Z _k based on the first to third observation data is expressed as a determinant as follows.

는, 제2 관측 데이터인 x좌표의 시각 t에서의 관측/센서 노이즈를 나타내고,

는 제2 관측 데이터인 y좌표의 시각 t에서의 관측/센서 노이즈를 나타내고,

는 제3 관측 데이터인 보폭의 시작 t에서의 관측/센서 노이즈를 나타내며,

는 제1 관측 데이터인 방위각의 시작 t에서의 관측/센서 노이즈를 나타낸다. 그리고 △는 k번째 스텝의 소요시간을 나타낸다. here

Represents the observation/sensor noise at time t of the x-coordinate, which is the second observation data,

Represents the observation/sensor noise at time t of the y-coordinate, which is the second observation data,

Represents the observation/sensor noise at the start t of the stride length, which is the third observation data,

Represents the observation/sensor noise at the start t of the azimuth angle, which is the first observation data. And Δ represents the required time of the kth step.

In this way, the current state is predicted based on the data collected from the satellite navigation device, the Doppler receiver, and the inertial sensor, and the process noise (error covariance predicted value) for determining the degree to be corrected based on this is predicted.

Thereafter, based on the predicted state value, the predicted error covariance value, and the predicted measurement value (estimated value) obtained in the above prediction process, a calibration process is performed.

First, Kalman gain K is calculated. Kalman gain is calculated based on process noise and observed noise by using the error between the predicted value and the actual measured value. Observed noise represents an actual measurement error, and process noise represents a prediction error.

은 프로세스 노이즈 즉, 위의 예측 과정에서 획득한 오차 공분산 예측값을 나타내며, R은 관측 노이즈 즉, 관측 노이즈 공분산을 나타낸다. here,

Represents the process noise, that is, the predicted error covariance obtained in the above prediction process, and R represents the observed noise, that is, the observed noise covariance.

를 산출한다. And by adding the observation/sensor noise W _k , the current state vector

Yields

는 현 상태에 대한 교정 직후의 예측값을 나타낸다. 위의 예측 과정에서

는

를 나타내며,

는

를 나타낸다. here,

Represents the predicted value immediately after correction for the current state. In the above prediction process

Is

Represents,

Is

Represents.

) 그리고, 예측 과정에서 획득한 상태 예측값을 토대로, 보정된 상태 예측값

를 획득한다. 이후 보정된 상태 예측값

을 토대로 한 측위가 이루어질 수 있다. In this way, the Kalman gain and the difference between the actual measured value and the predicted measured value (

) And, based on the state predicted value obtained in the prediction process, the corrected state predicted value

Get State predicted value corrected afterwards

Positioning based on can be made.

And, based on the Kalman gain, an estimated error covariance, which is a prediction error, is calculated.

As described above, the first observation data by the absolute positioning system and the second and third observation data, which are positioning results by the relative positioning system, are corrected/calibrated through the prediction process and the calibration process, as described above, More accurate positioning results can be obtained.

Measurements made by inertial sensors, including accelerometers and gyroscopes, always contain a systemic bias component. For example, the angular velocity by the gyroscope must be integrated to provide useful positioning information as a rotation angle. In this process, the contribution by the system bias component is always intervened. In the case of an accelerometer, the system bias component refers to a constant value indicating that the time average value does not become zero even in the case of constant velocity motion in which the accelerometer is not undergoing acceleration at all.In the case of a gyroscope, the gyroscope does not undergo rotation at all. Even in the case, the temporal average value does not become 0, but it means a constant constant value. Therefore, errors are continuously accumulated over time in the positioning information obtained indirectly from the accelerometer or gyroscope measurements.

It is known that these system biases of inertial sensors can mitigate the effect but never completely eliminate it. However, according to an embodiment of the present invention, when positioning is performed according to the tight coupling positioning method as described above, it is possible to provide an accurate positioning result by overcoming the problem of accumulation of errors inherent in a relative positioning system such as an inertial sensor.

4 and 5 are diagrams showing the structure of a tight coupling positioning device according to an embodiment of the present invention.

As shown in FIG. 4, the tightly coupled positioning device 1 according to an embodiment of the present invention receives first observation data from an inertial sensor 2 as a relative positioning device, and in particular, a satellite navigation device as an absolute positioning device. The second observation data is provided from (4), and the third observation data is provided from the Doppler receiving device (3).

To this end, the tightly coupled positioning device 1 inputs the first observation data from the inertial sensor 2 and the second observation data from the satellite navigation device 4 as shown in FIG. 5. A second input unit 12 receiving, a third input unit 13 receiving third observation data from the Doppler receiving device 3, and a tightly coupled positioning processing unit that performs tightly coupled positioning based on the first to third observation data ( 14).

The tightly coupled positioning processing unit 14 includes a variable setting unit 141 that sets a state variable for state prediction based on the first to third observed data, and a first state predicted value by predicting a current state based on the set state variable. A prediction processing unit 142, a second prediction processing unit 143 for predicting an error covariance corresponding to a current state, and an estimate value calculation unit 144 for calculating a predicted measurement value, which is an estimated value for observation data based on the state predicted value, In addition, a gain calculator 145 that calculates a gain (for example, Kalman gain) based on the predicted error covariance and the actual observed noise, and a state predicted value based on the calculated gain, the difference between the actual measured value and the predicted measured value. And an estimated value correcting unit 146 for correcting and calculating a new predicted value, that is, an estimated value, and an error calculating unit 147 for obtaining a corrected error covariance by correcting the predicted error covariance based on the calculated gain. Estimated values output from the estimated value correction unit 146 are used as data for later positioning.

6 is a flowchart of a tight coupling positioning method according to an embodiment of the present invention.

As shown in FIG. 6, the tightly coupled positioning device 1 according to an embodiment of the present invention receives data for positioning from the inertial sensor 2, the satellite navigation device 4, and the Doppler receiving device 3.

That is, the first input unit 11 receives first observation data, that is, azimuth angle information, from the inertial sensor 2 (S100), and the second input unit 12 receives second observation data from the satellite navigation device 4, that is, Pedestrian location information (x, y) is provided (S110). In addition, the third input unit 13 receives third observation data, that is, the pedestrian's stride length, from the Doppler receiving device 3 (S120). The third observation data includes an instantaneous speed for each step, and the instantaneous speed for each step may be calculated based on the stride length of the kth step and the required time Δ of the kth step.

Thereafter, the tightly coupled positioning processing unit 14 sets a state variable as in Equation 8 described above based on the first to third observation data (S130).

Next, the tightly coupled positioning processing unit 14 obtains a state predicted value by predicting the current state based on the set state variable (S140). Then, an error covariance predicted value is obtained by predicting an error covariance corresponding to a current state, and a predicted measurement value, which is an estimated value for the observed data, is obtained based on the state predicted value (S150). These steps (S130 to S150) correspond to the prediction process.

After the prediction process is finished, the tightly coupled positioning processing unit 14 performs a calibration process. Specifically, the tightly coupled positioning processing unit 14 calculates a gain (Kalman gain) based on an error covariance predicted value and an actual observed noise (S160), and calculates a state predicted value based on the calculated gain and the difference between the actual measured value and the predicted measured value. Corrected to obtain a corrected predicted value (estimated value) (S170). Here, the actual measured value is configured based on observed data (obtained from the sensor), that is, observed data provided from the first to third input units 11 to 13.

Meanwhile, the corrected error covariance is calculated by correcting the predicted error covariance based on the calculated gain.

It can be obtained (S180). These steps (S160 to S180) correspond to the calibration process.

The above prediction process and calibration process are performed so that the operation of each step is cyclically interlocked.

According to an embodiment of the present invention, more accurate positioning data is obtained by correcting/calibrating the first observation data by the relative positioning system as the second observation data and the third observation data, which are positioning results by the absolute positioning system. can do.

The embodiments of the present invention are not implemented only through the apparatus and/or method described above, but may be implemented through a program for realizing a function corresponding to the configuration of the embodiment of the present invention, a recording medium on which the program is recorded. Also, this implementation can be easily implemented by an expert in the technical field to which the present invention belongs from the description of the above-described embodiment.

Although the embodiments of the present invention have been described in detail above, the scope of the present invention is not limited thereto, and various modifications and improvements of the business operator using the basic concept of the present invention defined in the following claims are also provided. It belongs to the scope of rights.

Claims (15)

Setting a state variable for positioning based on the first observation data from the inertial sensor, the second observation data from the satellite navigation apparatus, and the third observation data from the Doppler receiving apparatus;
Performing a prediction process of calculating a state predicted value and an error covariance predicted value for the first to third observed data for a current state based on the set state variable; And
Compensating the predicted state value and the predicted error covariance value obtained in the prediction process with the difference between the measured value and the predicted value to obtain a new estimated value for positioning
Containing, the tight coupling positioning method. According to claim 1
The first observation data is azimuth information, a tightly coupled positioning method. According to claim 1
The second observation data includes pedestrian location information, and the location information comprises x-coordinates and y-coordinates. According to claim 1
The third observation data includes a stride length, a tightly coupled positioning method. According to claim 4
The third observation data includes the instantaneous speed for each step, the instantaneous speed for each step is calculated based on the stride length of the kth step and the required time △ of the kth step, and the required time △ of the kth step is the time k from the time of departure. A tightly coupled positioning method that is derived by dividing the total required time to by the number of steps detected by the accelerometer. The method according to any one of claims 1 to 5
The step of performing the prediction process,
Predicting a current state based on the state variable to obtain a state predicted value;
Predicting an error covariance corresponding to a current state to obtain a predicted error covariance value; And
Obtaining a predicted measurement value that is an estimated value for observation data based on the state predicted value
Containing, the tight coupling positioning method. According to claim 6
The step of performing the calibration process,
Calculating a gain based on the predicted error covariance and actual observed noise;
Correcting a state predicted value based on the calculated gain and a difference between an actual measured value and a predicted measured value to obtain a new estimated value; And
Obtaining a corrected error covariance by correcting the predicted error covariance value based on the calculated gain
Containing, the tight coupling positioning method. According to claim 7
The actual measured value is a first observation data from the inertial sensor, a second observation data from a satellite navigation apparatus, and a third observation data from a Doppler receiving apparatus. According to claim 7
The step of setting the state variable
And setting the state variable based on a new estimated value of a previous step obtained by performing the prediction process and the calibration process based on the first to third observation data. According to claim 1
The prediction process and the calibration process are performed based on a Kalman filter. A first input unit receiving first observation data from an inertial sensor;
A second input unit receiving second observation data from a satellite navigation device;
A third input unit receiving third observation data from the Doppler receiving device; And
Based on the first to third observed data, a state variable for positioning is set, a prediction process of calculating a predicted value and an error covariance predicted value for the current state is performed based on the set state variable, and the state obtained in the prediction process A tightly coupled positioning processing unit that obtains data for positioning by performing a calibration process to obtain a new estimate value for positioning by compensating the predicted value and the error covariance predicted value with the difference between the measured value and the predicted value
Containing, a tightly coupled positioning device. The method of claim 11
The first observation data is azimuth angle information, the second observation data includes pedestrian location information, the location information is composed of x-coordinates and y-coordinates, and the third observation data includes a stride length, a tightly coupled positioning device . The method of claim 12
The third observation data includes the instantaneous speed for each step, the instantaneous speed for each step is calculated based on the stride length of the kth step and the required time △ of the kth step, and the required time △ of the kth step is the time k from the time of departure. A tightly coupled positioning device derived by dividing the total required time to by the number of steps detected by the accelerometer. The method of claim 11
The tight coupling positioning processing unit
A variable setting unit configured to set the state variable based on a new state estimation value of a previous step obtained according to the prediction process and the calibration process based on the first to third observation data;
A first prediction processing unit that predicts a current state based on the state variable to obtain a state predicted value;
A second prediction processing unit that predicts an error covariance corresponding to a current state and obtains a predicted error covariance value; And
Estimated value calculation unit for obtaining a predicted measurement value, which is an estimate value for observation data based on the state predicted value
Containing, a tightly coupled positioning device. The method of claim 14
The tight coupling positioning processing unit
A gain calculator configured to calculate a gain based on the predicted error covariance and actual observed noise;
An estimated value correction unit for obtaining a new estimated value by correcting a state prediction value based on the calculated gain and a difference between an actual measured value and a predicted measured value;
An error calculator for obtaining a corrected error covariance by correcting the predicted error covariance based on the calculated gain
Further comprising a, tightly coupled positioning device.

Images