KR100748405B1 - Particle Sampling Method And Method For Sensor Fusion And Filtering - Google Patents

Abstract

The present invention relates to a technique for finding an estimate and a variance of each variable based on a constraint manifold. The method of sampling a particle using particles to filter and fuse uncertain data or information about a state variable of a system. For example, a sampling system is prepared by considering the effect of nonlinearity of constraints on system models, measurement models, or other systems on the probability distribution of state variables.

By using the sensor fusion and filtering method and system based on the constraint manifold as described above, degradation of the fusion and filtering performance can be reduced, Gaussian approximation error can be reduced, and mismatched information can be detected.

Sensor fusion, filtering, constraint manifold, marginal probability distribution, joint probability distribution

Description

Particle Sampling Method and Method For Sensor Fusion And Filtering

1 is a block diagram illustrating a sensor fusion and filtering process;

2 is a flowchart illustrating a sensor fusion and filtering method based on constraint manifolds according to the present invention;

3 is a conceptual diagram illustrating a sensor fusion concept in a linear constraint manifold according to an embodiment of the present invention;

4 is a conceptual diagram illustrating a sensor fusion concept in a nonlinear constraint manifold according to an embodiment of the present invention;

FIG. 5 is a diagram illustrating a sampling of a geometric manifold constituting a system model of Equation 10 according to the present invention.

6 is a diagram illustrating a sample distribution of according to the Gaussian samples of Equation 10 of the present invention.

FIG. 7 is a diagram illustrating a sample which is uniformly sampled in a geometric manifold constituting a measurement model of Equation 11 of the present invention;

8 is a graph showing the mean square error according to Example 1 and Comparative Example 1 of the present invention,

9 is a graph showing the mean square error according to Example 2 and Comparative Example 2 of the present invention,

10 is a graph showing the mean squared error according to Example 3 and Comparative Example 3 of the present invention,

11 is a graph showing the mean square error according to Example 4 and Comparative Example 4 of the present invention,

12 is a graph showing the mean square error according to Example 5 and Comparative Example 5 of the present invention;

13 is a graph showing the mean square error according to Example 6 and Comparative Example 6 of the present invention;

14 is a graph showing the mean square error according to Example 7 and Comparative Example 7 of the present invention;

15 illustrates a pattern of general sensor fusion.

The present invention relates to particle sampling methods and sensor fusion and filtering methods, and more particularly to techniques for determining the estimates and variances of each variable based on constraint manifolds.

In general, sensor fusion is an integrated process of combining or fusing different types of sensor information into one representative information data type.

15 is a diagram illustrating a pattern of general sensor fusion.

과

는 새로운 상위 대표값

으로 융합된다. 세번째 센서로 부터 얻어진 출력

은 융합 노드

와 융합되어 새로운 대표값

을 생성하게 되어 상위 노드와 융합이 이루어지게 된다.Figure 15 shows the outputs from the first two sensors

and

Is the new upper representative

To be fused. Output from third sensor

Silver fusion node

Fused with a new representative value

It generates a fusion with the parent node.

As shown in FIG. 1, the sensor fusion and filtering method is fused and filtered through two steps, a prediction 101 through a system model equation and a measurement 102 using a measurement equation. ) Conventional filters using such sensor fusion and filtering methods include Kalman filters, Extended Kalman filters, Unscented Kalman filters, Particle filters, and the like.

Among them, the Kalman filter, which is the basis for sensor fusion and filtering, is a linear filter suitable for Gaussian error model, but has a problem in processing a nonlinear system. An extended Kalman filter has been developed for processing such a nonlinear system, but an error due to linearization may occur, and thus the performance of the filter may vary according to the nonlinearity of the system. As an alternative to the Extended Kalman Filter, an Uncensored Kalman Filter was developed, which can handle nonlinear and Gaussian error model systems. This filter has been actively studied to date, but there is a problem that the performance of the filter may be degraded due to an incorrect Gaussian approximation.

Particle filters have been developed to overcome the problems of the conventional Kalman filter. Such a particle filter is a method of displaying a result of fusion of particles having various weights, and has an advantage that it can be applied to fusion and filtering of a system having ambiguous information as well as nonlinear and arbitrary error models. However, such a particle filter has a problem of real-time processing due to the failure of the prediction of the wrong particles and a large processing time.

For such sensor fusion and filtering, see [1] M. S. Grewal and A. P. Andrews, Kalman Filtering, 2nd ed. New York: Wiley, 2001. [2] H. W. Sorenson, Recursive Estimation for Nonlinear Dynamic Systems. New York: Marcel Dekker, 1988. [3] A. H. Jazwinski, Stochastic Processes and Filtering Theory. New York: Academic, 1970. [4] T. Ghirmai, N.F. Bugallo, J. Miguez and P.P. Djuri, "A novel particle filtering approach to blind symbol detection and timing estimation," IEEE 58th Vehicular Technology Conference (VTC2003), vol. 2, pp. 1147-1151, Oct. 2003. [5] D. Crisan and A. Doucet, "A survey of convergence results on particle filtering methods for practitioners," IEEE Transactions on Signal Processing, vol. 50, pp. 736-746, Mar. 2002. [6] J. H. Kotecha and P. M. Djuric, “Sequential monte carlo sampling detector for Rayleigh fast-fading channels,” in Proc. Int. Conf. Acoust., Speech, Signal Process., 2000. [7] A. Doucet, S. Godsill, and C. Andrieu, “On sequential Monte Carlo sampling methods for Bayesian filtering,” Statist. Comput., Vol. 10, no. 3, pp. 197-208. 2000. et al.

In contrast to the conventional method in which the sensor fusion and filtering method has two steps, the prediction through the model equation and the measurement using the measurement equation, the inventors obtain the probability of coupling on the constraint manifold defined in the unified fusion and filtering space. By simplifying the method, various constraints can be applied, and geometrical representations of constraints can be directly applied to various models, as well as fusion and filtering for nonlinear, non-Gaussian error models and systems with unclear information. We are also beginning to develop an integrated sensor fusion and filtering method that can be applied to.

An object of the present invention is to solve the problems described above, by applying a method for obtaining the probability of coupling on the constraint manifold defined in the unified fusion and filtering space, it is possible to apply a variety of constraints, geometrically It is to provide a sensor fusion and filtering method and system based on the constraint manifold that can be applied to various models by directly dealing with the expressed constraints.

Another object of the present invention is to express fusion manifolds in the fusion and filtering space without linearizing the system model and the observation model, thereby reducing sensor fusion and filtering performance due to incorrect linearization. It is to provide a method and system.

It is another object of the present invention to reduce the Gaussian approximation error, to detect the mismatched information, and to apply the convergence and filtering problems for a system having unclear information by calculating and calculating the error model as a sum of Gaussian. To provide a method and system for sensor fusion and filtering based on a variety of constraints.

In order to achieve the above object, the particle sampling method according to the present invention is a method of sampling a particle to filter and fuse uncertain data or information on a state variable of a system by using a particle, including a system model, a measurement model, or another system. It is characterized by sampling considering the effect of nonlinearity of the constraint on the probability distribution of state variables.

In the particle sampling method according to the present invention, the sampling method is a constant particle on the axis of the previous state variable to obtain a probability distribution of the current state variable estimated by the system model equation or another system model equation from the previous state variable. Sampling the current state variable particles and each current state variable particle according to a system model equation or another system model equation from a weight assigned by the sampled particles of the previous state variable and the prior probability information of the previous state variable. Obtaining an estimated weight, obtaining a prior probability distribution of the estimated current state from the sampled current state variable particles and the estimated weight of each current state variable particle, from the current state variable to obtain particles of the measurement variable The input of the measured variable estimated by the measured model equation Including the step of obtaining it is characterized in that formed.

In addition, in the particle sampling method according to the present invention, when the sampling method is defined as "Uniform sampling on Constraint Manifold", the sampling of the manifolds in a geometrical space of an arbitrary model equation is performed in advance. Sampling the particles of the previous state variable and the particles of the current state variable by sampling at constant intervals in the geometric space from a myriad of samples satisfying the system model equation; obtaining a weight of each current state variable particle estimated by the weights assigned by the particles of the previous state variable and the prior probability information of the previous state variable and the number of particles in the bucket; The sampled current state variable particles and each current state variable particle Obtaining the prior probability distribution of the estimated current state from the estimated weights, and sampling the particles of the current state variable and the particles of the measured variable by sampling at regular intervals in geometric space from a myriad of samples satisfying the measurement model equation in advance Characterized in that it comprises a step of obtaining.

In the particle sampling method according to the present invention, a sample satisfying the constraint manifold is obtained by non-linearity of the constraint manifold and the Monte Carlo method. .

In the particle sampling method according to the present invention, when the number of particles contained in each bucket is m, the weight of the particle of the current state variable is

Current State Variable Particle Weight = Original Weight in Each Bucket / m

It is characterized by obtaining by.

In addition, in order to achieve the above object, the fusion and filtering method according to the present invention is a method for fusion and filtering uncertain data or information on the state variables of the system using particles, the data consisting of the variables for the state and measurement or Setting a filtering and fusion space of information, defining a constraint manifold comprising various constraints in this space, calculating a joint probability distribution on the constraint manifold using the prior probability distribution of the variables, Fusion and filtering are performed, including calculating a marginal probability distribution of the variables from the combined probability distribution and obtaining an estimate and a variance of the variables.

The above and other objects and novel features of the present invention will become more apparent from the description of the specification and the accompanying drawings.

First, the concept of the present invention will be described.

As shown in FIG. 2, the sensor fusion and filtering method based on the constraint manifold according to the present invention sets a sensor fusion and filtering space composed of variables (S1), and the constraint manifold in the sensor fusion and filtering space. (S2), calculating a prior probability distribution of the variables (S3), calculating a coupling probability that rises above the constraint manifold from the prior probability distribution (S4), and back propagation to each variable axis from the coupling probability. Calculating a marginal probability of each variable (S5). This step is executed by a processor that processes multiple sensors and data collected by the multiple sensors. That is, a plurality of sensors used in the present invention are made of, for example, an ultraviolet sensor, an infrared sensor, a thermal sensor, and the like, but are not limited to a specific sensor type. In addition, a processor for processing data collected by a plurality of sensors is achieved by an ordinary computer system as being accompanied by a function capable of performing the above steps S1 to S5.

라 하고, 각 측정에 대한 추정치를

라 하며, 각 추정치 사이의 상관관계인 제약조건을

이라 하면, 하기 수학식 1와 같이 각 변수의 주변 확률 분포를 구하는 것을 고려해 볼 수 있다.To illustrate the sensor fusion and filtering scheme according to the present invention, the state and measurement of each variable

And estimates for each measurement

The constraint, which is the correlation between each estimate,

In this case, as shown in Equation 1 below, it may be considered to obtain a marginal probability distribution of each variable.

이고,

이며,

이다.Where

ego,

Is,

to be.

This process of the present invention is compared with the estimation of the Bayesian concept used in general sensor fusion and filtering.

값들의 추정은 하기 수학식 2의 사후 확률 분포(posterior probability distribution)를 구함으로써 얻을 수 있다.Bayesian concept

Estimation of the values can be obtained by obtaining a posterior probability distribution of Equation 2 below.

이다.In the above formula

to be.

의 주변 확률 분포를 구하는 것은 상기 수학식 2의 사후 확률 분포를 구하는 것과 동일함을 알 수 있다.Therefore, in the present invention, each variable

It can be seen that obtaining the neighbor probability distribution of is the same as obtaining the posterior probability distribution of Equation 2 above.

In the present invention, the process of obtaining the marginal probability distribution of Equation 1 is as follows.

로 구성된 센서 융합 및 필터링 공간

을 정의하고, 제약조건

을 설정한다.first,

Sensor fusion and filtering space

Define a constraint

Set.

와 분산이 각

축의 사전 확률로 나타나며, 센서 융합 및 필터링 공간

에는 각 사전 확률값의 곱으로 결합 확률 분포

가 표현된다.At this time, status and measurement

And scatter each

Sensor fusion and filtering space, represented by the prior probability of the axis

Has a combined probability distribution with the product of each prior probability value

Is expressed.

이 주어져 있으므로 이 공간에 제약조건을 기하학적으로 나타내고, 이 제약조건 상의 결합 확률 분포인

를 구한다.Above constraints

Since the constraints are geometrically represented in this space,

Obtain

The estimate of each variable and the updated variance are obtained by obtaining a marginal probability distribution that backpropagates the combined probability distribution on the constraint to each axis. Therefore, the sensor fusion and filtering method may be treated in the same manner as obtaining the marginal probability distribution as shown in Equation 1 above.

3 is a conceptual diagram illustrating a sensor fusion concept based on linear constraint manifolds according to an embodiment of the present invention. Referring to FIG. 3, the sensor fusion and filtering method according to the present invention will be described.

가 주어졌을 때 융합 결과인

를 추정하는 식은 하기 수학식 3과 같다.In general, in the Bayesian concept, two states and measurements,

Given the convergence result

The equation for estimating is equal to Equation 3 below.

과

가 서로 독립이라면 상기 수학식 3은 하기 수학식 4와 같이 간략하게 된다.Where status and measurement

and

If is independent of each other, the equation (3) is simplified as shown in equation (4).

은 정규화(normalization) 계수이다.Where

Is the normalization coefficient.

로 설정하고, 각 변수의 사전 확률 분포(303, 305)를 이용하여 상기 제약조건 위의 결합 확률 분포(304)를 구한 다음,

, 의 주변 확률 분포(302, 306)를 역전파를 통해 얻는다.As shown in Figure 3, for the sensor fusion and filtering according to the present invention, the constraint (301) as shown in Equation 5 below

Using the prior probability distribution of each variable (303, 305) to obtain the combined probability distribution 304 above the constraint,

The neighbor probability distributions 302 and 306 of, are obtained by back propagation.

이기 때문에, 적분을 풀어 다시 쓰면 하기 수학식 6과 같다.here,

For this reason, solving the integral and rewriting it is shown in Equation 6 below.

In the present invention, in order to show that calculating the probability of the surrounding probability is conceptually the same as the posterior probability distribution in general sensor fusion and filtering, the equation (6) is as follows.

로 정규화 계수이다.here,

Is the normalization coefficient.

과

가 서로 독립이라면 상기 수학식 7은 하기 수학식 8과 같이 간략화 된다.The above state and measurement

and

Are independent from each other, the equation (7) is simplified as shown in equation (8).

As such, Equations 7 and 8 derived according to the sensor fusion and filtering method of the present invention are consistent with Equations 3 and 4 derived from the general sensor fusion method.

Considering the constraint manifold according to the present invention as an arbitrary nonlinear function and extending to more general sensors and filtering methods, the problem of state estimation is described as follows.

과

시점의 변수의 실제 값과 추정치가

과

라 하고, 상태 전이 함수는

이라 하자.

and

The actual values and the estimates

and

State transition function

Let's say

를 측정한 센서의 출력값은

이 반면, 실제 상태 및 측정

와

는

를 만족한다고 가정하고,

과

에 연관된 불확실성이

와

로 주어진다고 가정하자.

The output value of the sensor measuring

On the other hand, real state and measurement

Wow

Is

Assume that we satisfy

and

The uncertainty associated with

Wow

Suppose is given by

는 다음의 과정으로 구해진다.At this time, the estimated value according to the present invention

Is obtained by the following procedure.

로 설정되고, 제약조건 다양체는

이다. 추정치

는

로 주어지며, 하기 수학식 9에 의하여 연산된다.Sensor fusion and filtering space

Set to, and the constraint manifold is

to be. Estimate

Is

It is given by Equation 9 below.

는 이미 알고 있는

와

로부터 구해진다.Where join probability

Already know

Wow

Obtained from

와

에 의한

의 전파로 얻어진 추정치

의 융합 문제는, 본 발명에 따라 상기 수학식 9와 같이 제약조건 만족의 문제로 풀 수 있다.As such, sensor readings

Wow

On by

Estimate obtained by propagation of

According to the present invention, the convergence problem can be solved as a problem of satisfying the constraint as shown in Equation (9).

4 is a conceptual diagram illustrating a sensor fusion concept in the nonlinear constraint manifold 401 according to an embodiment of the present invention. Referring to FIG. 4, a combined probability distribution 404 and a non-linear probability distribution 402 and 406 on nonlinear constraints may be represented using prior probability distributions 403 and 405 in a fusion and filtering space consisting of two variables.

In this case, even if the prior probability distributions 403 and 406 follow the Gaussian distribution, the peripheral probability distributions 402 and 406 obtained by back propagating from the combined probability distribution 404 on the constraint 401 have a non-Gaussian distribution.

Therefore, the present invention uses the sum of Gaussian to represent the non-Gaussian probability model.

According to the present invention, when the inconsistent information is fused, the combined probability distribution itself on the constraint manifold can be flattened to detect the inconsistency, and the unclear information is expressed and calculated as a probability having multiple peaks as a sum of Gaussian. Processing is possible without a single Gaussian approximation error.

Hereinafter, embodiments of the present invention will be described in detail.

In order to compare the sensor fusion and filtering performance according to the present invention with the conventional filtering performance, a UNV (Univariate Nonstationary Growth Model) having a system model of Equation 10 and a measurement model of Equation 11 was introduced. Constantly sampling the constraint manifolds reflected the geometric nonlinearity of the constraints in the filtering. It can be seen that in the geometric space of the constraint, samples corresponding to the samples on the manifold exist on each axis constituting the space, and these samples are arranged in accordance with the nonlinearity of the geometric feature. The sensor fusion and filtering method according to the present invention and state estimation simulations were compared with conventional extended Kalman filters, ascendant Kalman filters, and particle filters.

The experimental method according to the sampling method according to the present invention is as follows.

1. Sampling at regular intervals to a variety of geometric spaces in a given system model. The sampling at regular intervals on the manifolds in the geometrical space of the model equation is called "Uniform sampling on Constraint Manifold" and this is shown in FIG.

샘플들은 일정하게 배치되지 않는다.

축에 "Uniform sampling on Constraint Manifold" 간격보다 큰 간격으로 일정하게 나눈다. 이 간격 하나하나를 버킷(bucket)이라고 명한다. 각 버킷(bucket)안에 들어온 샘플들의 개수를 세고 이 개수를 m이라 한다. m에 따라 원래 지니고 있던 가중치들이 변하게 되는데 이를 변형된 가중치(modified weight)라 하며 다음과 같이 구한다. 2. Equivalent to "Uniform sampling on Constraint Manifold"

Samples are not consistently placed.

The axis is divided evenly into intervals larger than the "Uniform sampling on Constraint Manifold" interval. Each of these intervals is called a bucket. Count the number of samples in each bucket and call this m. Depending on m, the original weights are changed. This is called modified weight and is calculated as follows.

Transformed weights = weights originally possessed / m

에 해당되는

의 확률분포는 2번에서 구한 변형된 가중치를 통해 나타낸다.

의 확률 분포 결과(n=1 일 때)는 도 6과 같이 나타난다. 도 6에서 보는 바와 같이 시스템 모델식의 비선형성에 의해 확률분포가 나타남을 알 수 있다. 3.

For

The probability distribution of is represented by the modified weight obtained in step 2.

The probability distribution result (when n = 1) is shown in FIG. As shown in FIG. 6, it can be seen that the probability distribution is represented by nonlinearity of the system model equation.

의 확률분포를 구한다4. Go through the same process as 1 ~ 3 in the given measurement model

Find the probability distribution of

과 4 에서의 확률분포

를 곱한 뒤, 각 샘플값과 샘플에 해당되는 가중치에 대한 1차 모멘트를 구하여 최종 값을 구한다.5. Probability distribution at 3

Probability Distributions at and 4

After multiplying by, the first moment is obtained for each sample value and the weight corresponding to the sample to obtain the final value.

=

로 시스템 노이즈이며,

Where

=

System noise,

으로 설정하였다.

Set to.

=

로 측정 노이즈이다.Where

=

Measurement noise.

UNGM is a dynamic state space model with a very large nonlinear, bimodal feature.It is inaccurate for state estimation with conventional Extended Kalman Filters and Uncended Kalman Filters. Particle filters that follow Sequential Importance Sampling also show poor estimation performance when there are excessive measurement errors or non-Gaussian errors.

In the present invention, a sample satisfying the constraint manifold is obtained by nonlinearity of the constraint manifold and the Monte Carlo method.

<Example 1 and Comparative Example 1>

=

=

의 조건으로 모의 시험을 50 회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식(실시예 1), 익스텐디드 칼만 필터링 방식(비교예 1a), 언센티드 칼만 필터링 방식(비교예 1b), 및 파티클 필터링 방식(비교예 1c)의 평균 제곱 오차의 비교도이다. 도 8의 결과로부터, 본 발명에 따른 센서 융합 및 필터링 방식은 바이모달 시스템에 강인성을 나타냄을 알 수 있다.8 is

=

=

When the test was conducted 50 times under the conditions of, the sensor fusion and filtering method based on the constraint manifold (Example 1), the extended Kalman filtering method (Comparative Example 1a), and the ascended Kalman filtering method (Comparative Example 1b) And the mean square error of the particle filtering scheme (Comparative Example 1c). From the results of FIG. 8, it can be seen that the sensor fusion and filtering scheme according to the present invention exhibits robustness to the bimodal system.

<Example 2 and Comparative Example 2>

=

,

=

,

의 조건으로 모의 시험을 50회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식(실시예 2), 익스텐디드 칼만 필터링 방식(비교예 2a), 언센티드 칼만 필터링 방식(비교예 2b), 및 파티클 필터링 방식(비교예 2c)의 평균 제곱 오차의 비교도이다. 도 9의 결과로부터, 본 발명에 따른 센서 융합 및 필터링 방식은 비가우시안 노이즈에 대한 강인성을 나타냄을 알 수 있다.9 is

=

,

=

,

When the test was conducted 50 times under the conditions of, the sensor fusion and filtering method based on the constraint manifold (Example 2), the extended Kalman filtering method (Comparative Example 2a), and the ascended Kalman filtering method (Comparative Example 2b) And the mean square error of the particle filtering scheme (Comparative Example 2c). From the results of FIG. 9, it can be seen that the sensor fusion and filtering scheme according to the present invention exhibits robustness to non-Gaussian noise.

 <Example 3 and Comparative Example 3>

=

=

의 조건이지만, 융합 및 필터링에서는

=

의 잘못된 오차 모델을 가지고 모의 시험을 50회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식(실시예 3), 익스텐디드 칼만 필터링 방식(비교예 3a), 언센티드 칼만 필터링 방식(비교예 3b), 및 파티클 필터링 방식(비교예 3c)의 평균 제곱 오차의 비교도이다. 도 10의 결과로부 터, 본 발명에 따른 센서 융합 및 필터링 방식은 잘못된 오차 모델에 대하여 강인성을 나타냄을 알 수 있다.10 is

=

=

But in fusion and filtering

=

When 50 simulations were carried out with the wrong error model of, the sensor fusion and filtering method based on the constraint manifold (Example 3), the extended Kalman filtering method (Comparative Example 3a), and the ascended Kalman filtering method ( Comparative Example 3b), and a comparison of the mean square error of the particle filtering scheme (Comparative Example 3c). As a result of Figure 10, it can be seen that the sensor fusion and filtering method according to the present invention shows the robustness to the wrong error model.

<Example 4 and Comparative Example 4>

=

=

의 조건으로 모의 시험을 50회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식(실시예 4), 익스텐디드 칼만 필터링 방식(비교예 4a), 언센티드 칼만 필터링 방식(비교예 4b), 및 파티클 필터링 방식(비교예 4c)의 평균 제곱 오차의 비교도이다. 도 11의 결과로부터, 본 발명에 따른 센서 융합 및 필터링 방식은 절대적으로 큰 시스템 노이즈에 대하여 강인성을 나타냄을 알 수 있다.11,

=

=

When 50 simulations were carried out under the conditions of, the sensor fusion and filtering method based on the constraint manifold (Example 4), the extended Kalman filtering method (Comparative Example 4a), and the ascended Kalman filtering method (Comparative Example 4b) And the mean square error of the particle filtering scheme (Comparative Example 4c). From the results of FIG. 11, it can be seen that the sensor fusion and filtering scheme according to the present invention exhibits robustness against absolutely large system noise.

<Example 5 and Comparative Example 5>

=

=

의 조건이지만, 10번 중 1번은

으로 불명확한 정보를 주고, 모의 시험을 50회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식(실시예 5), 익스텐디드 칼만 필터링 방식(비교예 5a), 언센티드 칼만 필터링 방식(비교예 5b), 및 파티클 필터링 방식(비교예 5c)의 평균 제곱 오차의 비교도이다. 도 12의 결과로부터, 본 발명에 따른 센서 융합 및 필터링 방식은 불명확한 정보에 대한 강인성을 나타냄을 알 수 있다.12 is

=

=

Condition, but 1 out of 10

When unclear information is given and the simulation is conducted 50 times, the sensor fusion and filtering method based on the constraint manifold (Example 5), the extended Kalman filtering method (Comparative Example 5a), and the ascended Kalman filtering method (Comparative Example 5b) and the mean square error of the particle filtering method (Comparative Example 5c). From the results of FIG. 12, it can be seen that the sensor fusion and filtering scheme according to the present invention exhibits robustness against unclear information.

<Example 6 and Comparative Example 6>

=

=

의 조건이지만, 10번 중 1번은

으로 불일치 정보를 주고, 모의 시험을 50회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식(실시예 6), 익스텐디드 칼만 필터링 방식(비교예 6a), 언센티드 칼만 필터링 방식(비교예 6b), 및 파티클 필터링 방식(비교예 6c)의 평균 제곱 오차의 비교도이다. 도 13의 결과로부터, 본 발명에 따른 센서 융합 및 필터링 방식은 불일치 정보에 대한 강인성을 나타냄을 알 수 있다.13 is

=

=

Condition, but 1 out of 10

Inconsistent information is given, and when the simulation is conducted 50 times, the sensor fusion and filtering method based on the constraint manifold (Example 6), the extended Kalman filtering method (Comparative Example 6a), and the ascended Kalman filtering method ( Comparative Example 6b), and a comparison of the mean square error of the particle filtering scheme (Comparative Example 6c). From the results of FIG. 13, it can be seen that the sensor fusion and filtering scheme according to the present invention exhibits robustness against mismatch information.

<Example 7 and Comparative Example 7>

=

=

이지만 실제 시스템 오차 모델은

=

이며,

의 조건으로 모의 시험을 50회 실시하였을 때, 제약조건 다양체 기반의 센서 융합 및 필터링 방식 1(실시예 7)과 2(실시예 7a) 및 63개와 200개의 샘플을 사용한 파티클 필터링 방식(비교예 7과 비교에 7a)의 평균 제곱 오차의 비교도이다. 도 14의 결과로부터, 본 발명에 따른 센서 융합 및 필터링 방식은 파티클 필터에 비하여 잘못된 오차 모델에 대해 강인성을 나타냄을 알 수 있다.14 is an error model assumed in filtering

=

=

But the actual system error model

=

Is,

When 50 tests were conducted under the conditions of, the fusion manifold-based sensor fusion and filtering method 1 (Example 7) and 2 (Example 7a) and the particle filtering method using 63 and 200 samples (Comparative Example 7 Is a comparison of the mean square error of 7a). From the results of FIG. 14, it can be seen that the sensor fusion and filtering method according to the present invention exhibits robustness against a wrong error model compared to the particle filter.

From the above results, it can be seen that according to the sensor fusion and filtering method according to the present invention, by having the smallest mean square error for each evaluation condition, a robust sensor fusion and filtering method can be implemented for each evaluation condition.

As mentioned above, although the invention made by this inventor was demonstrated concretely according to the said Example, this invention is not limited to the said Example and can be variously changed in the range which does not deviate from the summary.

As described above, according to the method and system for sensor fusion and filtering based on the constraint manifold according to the present invention, by applying a method for obtaining the coupling probability on the constraint manifold defined in the unified fusion and filtering space, It can be applied and applied to various models by directly dealing with geometrically expressed constraints, and fusion and filtering due to incorrect linearization by expressing constraint model in the fusion and filtering space without linearizing system model and observation model. The effect is that it can reduce performance degradation, reduce Gaussian approximation errors, detect mismatched information, and apply it to fusion and filtering problems for systems with indeterminate information.

Claims (6)

In systems that use particles to filter and fuse uncertain data or information about the state variables in the system, sampling the nonlinearities of the system model, measurement model, or other constraints that the system has on the probability distribution of the state variables As a way to, Sampling particles in a uniform distribution over a system model, measurement model, or other constraints the system has, or intentionally sampling a large number of particles in an area that significantly affects the posterior probability distribution of system state variables, or a previous state. Sampling particles uniformly along the axis of the variable, After the particle sampling step, Obtaining estimated weights of current state variable particles and respective current state variable particles according to the system model equation or another system model equation, Obtaining a prior probability distribution of an estimated current state from the sampled current state variable particles and estimated weights of each current state variable particle, Obtaining particles of the measurement variable estimated by the measurement model equation from the current state variable. delete The method of claim 1, The sampling method is A constant interval in the geometric space from a sample that satisfies the system model equation or other system model equations when defined as "Uniform sampling on Constraint Manifold" is a sampling of the manifolds in the geometric space of an arbitrary model equation. Sampling to obtain the particles of the previous state variable and the particles of the current state variable, When a constant division in the axis of the previous state variable is defined as a bucket, the weight assigned by the particles of the previous state variable and the prior probability information of the previous state variable and the number of particles contained in the bucket are determined. Obtaining a weight of each estimated current state variable particle, Obtaining a prior probability distribution of an estimated current state from the sampled current state variable particles and estimated weights of each current state variable particle, And obtaining the particles of the current state variable and the particles of the measurement variable by sampling at regular intervals in the geometric space from samples satisfying the measurement model equation in advance. The method of claim 3, Non-linearity of the constraint manifolds of the system model, measurement model or other system and a sample that satisfies the constraint manifolds by Monte Carlo method. The method of claim 3, When the number of particles in each bucket is m, the weight of the particles of the current state variable is Current State Variable Particle Weight = Original Weight in Each Bucket / m The particle sampling method characterized by the above-mentioned. A method of using particles to fuse and filter uncertain data or information about a state variable of a system, Establishing a filtering and fusion space of data or information consisting of variables for state and measurement, Defining a constraint manifold containing various constraints in this space, Calculating a combined probability distribution on the constraint manifold using a prior probability distribution of the variables, Calculating a peripheral probability distribution of the variables from the combined probability distribution; and Fusing and filtering is performed including obtaining an estimate and a variance of said variables.

Images