Problem-Oriented Approach to Identification of Sensor Error Models and its Application to Navigation Data Processing

Proceedings of the 20th World The International Federation of Congress Automatic Control Proceedings of the 20th9-14, World The International Federation of Congress Automatic Control Toulouse, France, July 2017 Proceedings of the 20th9-14, World The International Federation of Congress Automatic Control Toulouse, France, July 2017 Available online at www.sciencedirect.com Proceedings of the 20th World Congress The International of Automatic Control Toulouse, France,Federation July 9-14, 2017 The International of Automatic Control Toulouse, France,Federation July 9-14, 2017 Toulouse, France, July 9-14, 2017

ScienceDirect

Problem-Oriented Approach to50-1Identification of Sensor Error Models IFAC PapersOnLine (2017) 2830–2835 Problem-Oriented Approach to Identification of Sensor Error Models Problem-Oriented Approach to Identification of Sensor Error Models and its Application Navigation Data Processing and its Application Navigation Data Processing Problem-Oriented Approach to Identification of Sensor Error Models Problem-Oriented Approach to Identification of Sensor Error Models and its Application Navigation Data Processing , and its Application to Navigation Data Processing O.A. Stepanov* ,**, A.V. Motorin** and its Application to Navigation Data Processing O.A. Stepanov* **, A.V. Motorin** , 

O.A. Stepanov*,**,  A.V. Motorin** O.A. Stepanov*,**,  A.V. Motorin** O.A. Stepanov* **, *CSRI Elektropribor, JSC  A.V. Motorin** *CSRIRussia Elektropribor, JSC (e-mail: [email protected]) Saint Petersburg, *CSRIRussia Elektropribor, JSC Saint Petersburg, (e-mail: [email protected]) **ITMO University *CSRI Elektropribor, JSC Saint Petersburg, Russia (e-mail: [email protected]) **ITMO University *CSRI Elektropribor, JSC SaintSaint Petersburg, Russia (e-mail: [email protected]) Petersburg, Russia (e-mail: [email protected]) **ITMO University SaintSaint Petersburg, Russia (e-mail: [email protected]) Petersburg, Russia (e-mail: [email protected]) **ITMO University Saint Petersburg, Russia (e-mail: [email protected]) **ITMO University Saint Petersburg, Russia (e-mail: [email protected]) Saint Petersburg, Russia (e-mail: [email protected]) Abstract: The paper proposes a problem-oriented approach to identification of error models of navigation Abstract: The on paper proposes aofproblem-oriented approach to identification of error models problem. of navigation sensors based the solution a joint hypotheses recognition and parameter estimation The Abstract: The paper proposes a problem-oriented approach to identification of error models problem. of navigation sensors based on the solution of a joint hypotheses recognition and parameter estimation The examples illustrating the proposed approach demonstrate that the obtained algorithms outperform the Abstract: The paper proposes a problem-oriented approach to identification of error models of navigation sensors based on the proposes solution ofproblem-oriented a joint hypotheses recognition andobtained parameter estimation problem. The examples illustrating the proposed approach demonstrate that the algorithms outperform the Abstract: The paper a approach to identification of error models of navigation existing ones. sensors based on the solution of a joint hypotheses recognition and parameter estimation problem. The examples illustrating the proposed approach demonstrate that the algorithms the existing ones. sensors based on the solution of a joint hypotheses recognition andobtained parameter estimationoutperform problem. The examples illustrating the proposed approach demonstrate that the obtained algorithms outperform the existing ones. © 2017, IFAC (International Federation of Automatic Control) Hosting Elsevier Ltd. All rights reserved. the examples illustrating the proposed approach demonstrate that the by obtained algorithms outperform Keywords: Stochastic system identification, Bayesian methods, estimation and filtering. existing ones. Keywords: Stochastic system identification, Bayesian methods, estimation and filtering. existing ones. Keywords: Stochastic system identification, Bayesian  methods, estimation and filtering. Keywords: Stochastic system identification, Bayesian estimation and filtering.  methods, 2015). The paper and is structured Keywords: Stochastic system identification, Bayesian methods, estimation filtering. as follows. First, Section 2  1. INTRODUCTION 2015). The is structured as follows. First, Section 2 considers thepaper theoretical framework of the research and the  1. INTRODUCTION 2015). The paper is structured as follows. First, Section 2 considers the theoretical framework of the research and the  INTRODUCTION algorithm forpaper solving the framework above as problem. Section presents 2015). Thethe is structured follows. First, 3Section 2 One of the ways to1. enhance the accuracy and reliability of considers theoretical of the research and the INTRODUCTION algorithm forpaper solving theconfirming above as problem. presents Thethe is structured follows. First, 3Section 2 One of the waysnavigation to1. the accuracy and reliability of 2015). the simulation results thetheSection efficiency of the considers framework of research and 1.enhance INTRODUCTION modern onboard systems is to improve algorithms algorithm for theoretical solving theconfirming above problem. Section 3 presents One of the ways to enhance the accuracy and reliability of the simulation results the efficiency of the considers the theoretical framework of the research and modern onboard is to improve algorithms proposed algorithm. Inthe Section we show how the3algorithm for solving above4 problem. presents for processing measurements from various sensors. One of the waysnavigation to enhancesystems the accuracy and reliability of algorithm the simulation results confirming the Section efficiency of the modern onboard navigation systems is to improve algorithms proposed algorithm. Inapplication Section we show how the3algorithm algorithm for solving the above4 problem. Section presents for processing measurements from various sensors. One of the ways to enhance the accuracy and reliability of is used to solve some problems of navigation the simulation results confirming the efficiency of data the Traditionally, data fusion in these systems is implemented modern onboard navigation systems is to improve algorithms proposed algorithm. In Section 4 we show how the algorithm for processing measurements from various sensors. the is used to algorithm. solve some application problems of navigation simulation results confirming the with efficiency of data the Traditionally, data fusion in these systems is implemented modern onboard navigation systems is to improve algorithms processing and our results are compared those obtained proposed In Section 4 we show how the algorithm using stochastic filtering algorithms (Gibbs 2011, Stepanov, for processing measurements various sensors. proposed is used to algorithm. solve some application problems of navigation data Traditionally, data fusionalgorithms in these from systems is implemented processing and our results are compared with those obtained In Section 4 we show how the algorithm using stochastic filtering (Gibbs 2011, Stepanov, for processing measurements from various sensors. by the Allan variance. is used to solve some application problemswith of navigation data 2016). To design them, signals errors2011, navigation Traditionally, data fusionalgorithms in theseand systems is ofimplemented processing and our results are compared those obtained using stochastic filtering (Gibbs Stepanov, by the Allan variance. is used to solve some application problemswith of navigation data 2016). To design them, signals and errors2011, ofimplemented navigation Traditionally, data fusion in these systems issequences processing and our results are compared those obtained sensors should be described as random or using stochastic filtering algorithms (Gibbs Stepanov, by the Allanand variance. 2016).stochastic To design them, signalsasand errors2011, of navigation processing our results are compared with those obtained sensors should be described random sequences or using filtering algorithms (Gibbs Stepanov, by the Allan variance. 2. PROBLEM-ORIENTED APPROACH TO processes, which, turn,signals makes itrandom necessary identify 2016). designbein them, errors sequences of to navigation sensors To should described asand or by the Allan variance. OF SENSOR 2. PROBLEM-ORIENTED APPROACH TO processes, which, in turn, makes itrandom necessary identify 2016). To designbe them, signals and errorsobtained of to navigation IDENTIFICATION ERROR MODELS models of sensor errors. The latter are often by the sensors should described as sequences or 2. PROBLEM-ORIENTED APPROACH TO processes, which, in turn, makes it necessary to identify IDENTIFICATION OF SENSOR ERROR MODELS models ofbased sensor errors. Themakes latter are oftenspectral obtained by the sensors should be described as random sequences or 2. PROBLEM-ORIENTED APPROACH TO methods on the calculation of power densities processes, which, in turn, it necessary to identify IDENTIFICATION OF SENSOR ERROR MODELS models ofbased sensor errors. Themakes latter are oftenspectral obtained by the As it was2.noted PROBLEM-ORIENTED APPROACH in Introduction, algorithms usedMODELS inTO navigation methods on errors. the processes, which, in calculation turn, itpower necessary toofdensities identify OF SENSOR ERROR (PSD) and/or correlation functions using samples sensor models ofbased sensor The latterof are often obtained by the As itIDENTIFICATION was noted in Introduction, algorithms usedMODELS in navigation methods on the calculation of power spectral densities IDENTIFICATION OF SENSOR ERROR data processing involve a description of sensor errors in the (PSD) and/or correlation functions using samples of sensor models ofbased sensor errors. The latter(Bendat are often obtained by the As it was noted in Introduction, algorithms used in navigation errors and signals to be estimated and Piersol, 2010). methods on the calculation of power spectral densities data processing involve a description of sensor errors in (PSD) and and/or correlation functions using samples ofdensities sensor As form of a shaping filter. Assume that we can derive a setthe of it was noted in Introduction, algorithms used in navigation errors signals to be estimated (Bendat and Piersol, 2010). methods based on the calculation of power spectral datait processing involve aAssume description of sensor errors in the These methods are limited only to ergodic processes. (PSD) and/or correlation functions using samples of sensor form of a shaping filter. that we can derive a setthe of As was noted in Introduction, algorithms used in navigation errors and signals to be estimated (Bendat and Piersol, 2010). hypotheses about a sensor error model, each of which can be data processing involve a description of sensor errors in These methods are limited only to ergodic processes. (PSD) and/or correlation functions using samples of sensor form processing of a shaping filter. Assume that of we canofderive acan setthe of Recently, Allan variance has been extensively to identify errors and signals to belimited estimated (Bendat andused Piersol, 2010). data hypotheses about a sensor error model, each which be involve a description sensor errors in These methods are only to ergodic processes. represented as a general Markov sequence: form of a shaping filter. Assume that we canofderive acan set be of Recently, Allan variance has This been extensively used to identify errors and signals tosensors. belimited estimated (Bendat and Piersol, 2010). hypotheses about a sensor error model, each which errors of navigation method is also applicable to These methods are only to ergodic processes. represented as a general Markov form of a shaping Assume that we canofderive set be of Recently, Allan variance has been extensively used to identify about a filter. sensor error sequence: model, each whichacan errors of navigation method is also applicable to hypotheses These methods aresensors. limited only to 2015, ergodic processes. represented ask a general sequence: k k k about k k Markov kerror model, some nonstationary processes (Allan Stepanov and Recently, Allan variance has This been extensively used to identify hypotheses a sensor each of which can be     x (θ ) x Г (θ ) w , errors of navigation sensors. This method is also applicable to  represented general some nonstationary processes 2015, Stepanov and Recently, Allan variance has been(Allan extensively tomethods identify  sequence: xiikk  k iikk (θ kk )as xika11  Гiikk (θ kk Markov ) wiikk ,  (1) Motorin 2014, Krobka, 2015). However, allused the errors navigation sensors. This method is also applicable to represented  as general Markov sequence: some of nonstationary processes (Allan 2015, Stepanov and k ika (1) Motorin 2014, Krobka, 2015). However, all the methods  errors of navigation sensors. This method is also applicable to   x (θ ) x Г (θ ) w ,   θ θ θ ,  ik ik ik  k mentioned above involve long-term measurements and ikik1 k k ik1  some nonstationary processes (Allan 2015, Stepanov  (1) Motorin 2014, Krobka, 2015). However, all Stepanov the methods θxikk  θikik1(θkθ)kx,ik1  Гik (θ k ) wik ,   mentioned above involve long-term measurements and  some nonstationary processes (Allan 2015, k k k k k k  complicated accuracy estimation, this their (1) Motorin Krobka, 2015). However, all being the methods kθ))kxx,iik1Гiki ((θ θxyiiki  H θikiki1((θ    ))vwi, ,  (2) mentioned2014, above involve long-term measurements and  complicated accuracy estimation, this being their (1) Motorin 2014, Krobka, 2015). However, all that the methods  θ)kx,i  i (k )viik ,  θyiki  H θiki 1(  (2) disadvantages. It should also be emphasized in these mentioned above involve long-term measurements and  k k k k k k complicated above accuracy estimation, this beingin these their  θ) x,   ( )v ,  θyi  H θi 1(k disadvantages. It should also be emphasized that mentioned involve long-term measurements and  (2) i i i i i k k k k k k x methods the structure of model is generally determined where is the error model state vector; is the k  1. K complicated accuracy estimation, this that beingin these their yi  Hikx(ikk )isxik the  ik (k )vmodel (2) disadvantages. It shouldofalso be emphasized ik , methods the In structure model istype generally determined where state vector; k  1.K is the complicated accuracy estimation, thisof that being their ik ) x  error yi  Hi x(of (2) empirically. words, process and where disadvantages. It other shouldof also be aemphasized in these i the i ( )vmodel i ,that specifies number the hypothesis the structure of the is error state vector; is methods the structure model is generally determined k  1. K ik empirically. In words, aemphasized type of that process and where disadvantages. It other should also be in these number xof the hypothesis that specifies the structure of the corresponding PSD as well as correlation function are often k k k k k k k k methods the structure of model is generally determined is the error model state vector; is the k  1. K ik empirically. In other words, a type of process and sensor ik (θ , Гspecifies , Hthe are numbererror the hypothesis corresponding PSD as well as correlation are often k) ik (θ kof ik (θ kstructure ik (θ k ) vector; xof methods structure of model generally determined ismodel; the error model is)) the k)) ,,  1.K i chosen to the describe sensor errors without anyfunction formal criterions. ik (θ )that  sensor error model; , Гstate are empirically. In other words, a istype of process and where ik (θ kof ik (θ kstructure ik (θ k ) , Hthe number of the hypothesis the k that specifies corresponding PSD as well as correlation function are often chosen to describe sensor errors without anyfunction formal criterions. the matrices describing filter for empirically. InPSD other words, a obtained type of process and ik (θthe )that ) , this  ik (θmodel, ) the sensor error model; Гspecifies , shaping are number of the hypothesis of ik (θ kstructure ik (θ k ) , Hthe Moreover, in a number of cases the model cannot be corresponding as well as correlation are often k k the matrices describing the shaping for, this model, chosen to describe sensor errors without anyfunction formal criterions. dependent Hvector  sensor error model; , filter Moreover, in a number of cases the obtained model cannot be which are nonlinearly on parameters corresponding PSD as well as correlation are often k )the ik (θ k ) , Г ik (θ ik (θ k ) are ik (θ k ) of the matrices describing for, this model, directly tosensor Bayesian filtering chosen toapplied describe without any formal criterions. dependent (θthe ) , shaping Hvector  Гki (θ error model; , filter which are nonlinearly on )the parameters i (θ ) are i (θ ) of Moreover, in a number oferrors casesstochastic the obtained modelalgorithms cannot be sensor k ki directly tosensor Bayesian stochastic filtering algorithms chosen toapplied describe errors without any formal criterions. the the shaping filter forofsequences this model, - dimensional of wikkareand vdescribing θ kk ; matrices which nonlinearly the vector parameters k and m k on ik are pdependent that imply the description of errors in time domain using the Moreover, in a number of cases the obtained model cannot be the matrices describing the shaping filter for this model, directly applied to Bayesian stochastic filtering algorithms and m k on - dimensional sequences of vik are pdependent θ k ; wikareand that imply the description of errors in time domain using the Moreover, in a number of cases the obtained model cannot be which nonlinearly the vector of parameters k shaping filter in state the and noise, which are of discrete zerodirectly applied to Bayesian stochastic filtering are pdependent and m - dimensional sequences of wikareand vik measurement θ k ; system nonlinearly vector parameters that imply thedefined description of space. errors in time domainalgorithms using the which k on the k shaping filter defined in state space. the system and measurement noise, which are discrete zerodirectly applied to Bayesian stochastic filtering algorithms m k identity are pnoise - dimensional sequences of θ k ; wGaussian mean covariance matrices. k and ik and vik white that imply thedefined description of space. errors in time domain using the the and measurement noise, which are discrete zeroshaping filter in state m are pnoise andwith - dimensional sequences of wGaussian θ ; system with identity covariance matrices. i and vi white that imply the description of space. errorstoinidentification time domainofusing the mean The paper proposes an approach models Note that for different hypotheses the dimensionality of shaping filter defined in state the system and measurement noise, which are discrete zeroThe paper proposes an approach to estimated, identification of models mean Gaussian white noise with identity covariance matrices. Note that for different hypotheses the dimensionality of shaping filter defined in state space. the system and measurement noise, which are discrete zerofor sensor errors and signals to be which is free k mean noisehypotheses with identity matrices. The sensor paper errors proposes ansignals approach to identification of models xdifferent vectorsGaussian be different. θ kk and for andan to be is free that for white thecovariance dimensionality of ik can also mean Gaussian white noisehypotheses with identity covariance matrices. from the above-mentioned disadvantages. It iswhich intended to Note The paper proposes approach to estimated, identification of models xdifferent vectors be different. θ k and ik can also Note that for the dimensionality of for sensor errors and signals to be estimated, which is free from the above-mentioned disadvantages. It is intended to The paper proposes an approach to identification of models be different.the dimensionality of vectors θ k and Note that for xdifferent hypotheses solve the above-mentioned problem of signals navigation processing in iswhich for sensor errors and to bedata estimated, which free ik can also from the disadvantages. It is intended to The problem of model identification can be formulated as a vectors θ k and xik can also be different. solve the problem ofdescribed navigation processing in iswhich for sensor errors and signals toby bedata free errors and above-mentioned signals are a estimated, shaping defined in The problem model identification can be formulated as a from the disadvantages. It filter iswhich intended to vectors can also be different. θ andofxof i solve the problem of navigation data processing in which joint problem hypotheses recognition and parameter errors and above-mentioned signals by data a2016), shaping defined from the disadvantages. It filter is intended to The problem of model identification can be formulated as a state (Gibbsare 2011, Stepanov which whywhich it in is joint problem hypotheses and parameter solve the ofdescribed navigation processing in errorsspace and problem signals are described by data a2016), shaping filteris defined in estimation, i.e., the hypotheses problem of recognition determining the hypothesis The problem of of model identification can be formulated as a state space (Gibbs 2011, Stepanov which is why it is solve the problem of navigation processing in which joint problem of recognition and parameter called problem-oriented. The new research isfilter based on itthe errors and signals described by a2016), shaping in estimation, i.e., the problem of recognition determining the hypothesis The problem of model can for be formulated as a state space (Gibbsare 2011, Stepanov which isdefined why is estimation, number k, i.e., which isidentification bestof suited measurements joint problem of and parameter called problem-oriented. The new research isfilter based on the errors and signals are described by a2016), shaping defined in the hypotheses problem determining the hypothesis previous results published in (Stepanov and Motorin 2014, state space (Gibbs 2011, Stepanov which is why it is number k, which is best suited for measurements joint problem of hypotheses recognition and parameter calledspace problem-oriented. The new research isMotorin based on the estimation,k, i.e., the problem determining hypothesis previous results in new (Stepanov and 2014, state (Gibbspublished 2011, Stepanov 2016), which is why itthe is number which is bestof suited for the measurements called problem-oriented. research based on estimation, the problem determining hypothesis previous results publishedThe in new (Stepanov andis Motorin 2014, number k, i.e., which is bestof suited for the measurements called problem-oriented. The research is based on the previous results published in (Stepanov and Motorin 2014,2885number k, which is best suited for measurements Copyright © 2017 IFAC previous published in (Stepanov and Motorin 2014,2885 Copyright results © 2017 IFAC 2405-8963 © 2017, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright 2017 responsibility IFAC 2885Control. Peer review©under of International Federation of Automatic Copyright © 2017 IFAC 2885 10.1016/j.ifacol.2017.08.635 Copyright © 2017 IFAC 2885

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 O.A. Stepanov et al. / IFAC PapersOnLine 50-1 (2017) 2830–2835

2831

Yi   y1 ... yi  , and estimation of the corresponding

k , fk  k / Yi 1 , H  hk  is the posterior PDF for θ k at time

vectors θ k and xik . This problem statement has many similarities with the statement of the tracking problem (Beloglazov and Kazarin 1998, Bar-Shalom et.al. 2011), which can be solved by the multiple model approach (Magill 1965, Dmitriev and Stepanov 2001, Sarkka 2013). Let us describe the algorithm for solution of the problem under consideration and show the results of its application to identification of models for navigation sensor errors.

step i–1. For the PDF of state vector xik we can write

T

Assume that a set of hypotheses is treated as a random variable H taking values hk , where k  1...K , K is the total number of hypotheses. Its probability density function (PDF) can be represented as in (Dmitriev and Stepanov 2001): K

f H  H    Pr(H  hk )(H  hk ) ,

(3)

f xk  xik / Yi , H  hk   i

  f xk  xik / Yi , H  hk , k  f k  k / Yi , H  hk  d k . i

A distinctive feature of the problem lies in the fact that for the fixed hypothesis and fixed k , model (1), (2) specifies a linear Gaussian filtering problem. Therefore, the likelihood functions f yi  yi / Yi 1 , H  hk ,k  kj   N  yi ;H ikj xˆikj/ i 1 , ikj  , f xk  xik / Yi , H  hk , k  kj   N  xik ; xˆikj , Pi kj  ,

are also Gaussian. Here  kj are the possible values of vector k , j  1...M k , M k is the total number of these values;

where Pr(H  hk ) is the prior probability that the hypothesis k

k 1

estimate for vectors k and xik as ˆ ik (Yi )   k f k  k / Yi , H  hk  d k ,    k k k k xˆi (Yi )   xi f xk  xi / Yi , H  hk  dxi ,  i  fk  k / Yi , H  hk 

and

(4) f xk  xik / Yi , H  hk 

H ikj xˆikj/ i 1 and covariances Pi kj , ikj  Hikj Pi kj/ i 1 ( Hikj )T  ikj (ikj )T .

For a

fixed hypothesis H  hk it is possible to find the Bayesian

where

N  •  defines the Gaussian functions with expectations xˆikj ,

K

 Pr(H  h )  1 .

The optimal estimate and prediction xˆikj , xˆikj/ i 1 with their covariances Pi kj , Pi kj/ i 1 are obtained using a Kalman filter bank. Taking into account (8), to calculate the integrals (4), we only need an approximation for PDF fk  k / Yi , H  hk  . In this case we can choose, for example, the point mass approximation: Mk

f k  k / Yi , H  hk    ikj   k  kj ,

are

i

posterior PDF for k and xik , correspondingly, at fixed hypothesis H  hk . For optimal estimates we choose the

j 1

f k   / H  hk          . k

hk

Therefore, to solve the problem of identification for model (1), (2), we need to know the PDFs f H  H / Yi  ,

ikj 

Using the Bayes theorem, we can represent the posterior hypothesis probability as

 f y

i

/ Yi 1 , H  hk  f H  H  hk / Yi 1 

kj

ikj1  f yi  yi / Yi 1 ,H  hk , kj  L



kj i 1

j 1

i

K

k

According to the Bayes theorem, the following recurrence relation for coefficients ikj of approximation (10) holds true:

fk  k / Yi , H  hk  , f xk  xik / Yi , H  hk  .

Pr(H  hk / Yi ) 

kj 0

j 1

(5)

f  yi / Yi 1 , H  hk  f H  H  hk / Yi 1 

(10)

Mk

estimates that maximize the posterior PDF f H  H / Yi  : hk*  arg max f H  H / Yi  .

(9)

i

k 1

of the model H  hk is true, thus

(8)

f yi  yi / Yi 1 ,H  hk , 

kj



.

(11)

Substituting (10) into (7), the required integrals (4) and probability (6) can be calculated using the equations:

, (6)

Mk

Mk

j 1

j 1

ˆ ik (Yi )   ikj kj , xˆik (Yi )   ikj xˆikj ,

(12)

Pr(H  hk / Yi ) 

k 1

 M k kj kj kj kj  where f  yi / Yi 1 , H  hk  is the likelihood function of the   i 1 N  yi ;H i xˆi / i 1 ;  i   Pr(H  hk / Yi 1 ) . (13) j 1   measurement for the fixed hypothesis at time step i, which can  K  Mk be represented as   kj kj kj kj    i 1 N  yi ;H i xˆi / i 1 ;  i   Pr(H  hk / Yi 1 )   k 1  j 1     f  yi / Yi 1 , H  hk   The considered method for calculation of the optimal estimate (7)   f yi  yi / Yi 1 , H  hk ,k  f k  k / Yi 1 , H  hk  d k , based on formula (8) is called the partitioning method (Lainiotis, 1976). Note that (10) can replaced by an k where f yi  yi / Yi 1 , H  hk ,  is the likelihood function of the approximation based on particle filters. In this case, the described technique is better known as the Raomeasurement for the fixed hypothesis and parameter vector 2886

Proceedings of the 20th IFAC World Congress 2832 O.A. Stepanov et al. / IFAC PapersOnLine 50-1 (2017) 2830–2835 Toulouse, France, July 9-14, 2017

Blackwellization procedure (Doucet, Freitas, and Gordon 2001). An important advantage of the Bayesian approach is the fact that we can calculate the covariance matrices for estimates (12) as  Pi (Yi )         ˆ ˆ ,  j 1  (14)  Mk T xk kj kj kj T kj k k  Pi (Yi )   i  xˆi ( xˆi )  Pi    xˆi  xˆi  .   j 1  In other words, we can calculate the accuracy characteristics for the estimates of the state and parameter vectors corresponding to measurements Yi . Besides, it is possible to determine the potential accuracy of parameter estimation by introducing the unconditional covariance matrix: Mk

k

kj i

kj T

kj



k i

  k i



(15)

  (k  ˆ k )(k  ˆ k )T f (k , Yi / H  hk )d k dYi ,

where f (k , Yi / H  hk ) is the joint PDF for vectors θ k and Yi for the fixed hypothesis. With the estimation algorithm for ˆ k (Y ) , the diagonal elements of this matrix can be calculated i

measurement sample Yi l , it will be called a ‘mean calculated probability of correct identification’, and value err Pr i (H  hk ) 

T

Gi  E (k  ˆ k )(k  ˆ k )T  k

the hypothesis H  hk corresponds to the true hypothesis for

A

err Pr i (H  hk ) 







(16)

where θ k (l ) is a set of samples of parameter vectors with PDF f (k ) , and θˆ k (Y l ) , l  1.L is a set of the estimates i

obtained with the use of (12) and measurements Yi l , formed in accordance with model (1), (2). It is the usual practice that matrix (15) is also calculated with the use of another relation: Gik   Pi k (Yi ) f (Yi )dY  GiCk 

1 l  L k l  Pi (Yi ) , L l 1

(17)

where Pi k (Yi l ) is the covariance matrix calculated using (14). The coincidence of matrix GiRk , hereinafter called a ‘real’ covariance matrix, with matrix GiCk called a ‘calculated’ matrix is an evidence in favor of the validity of the results obtained. The same matrices can be defined for state vector xˆik . The possibility of calculating the matrices that characterize the potential estimation accuracy quantitatively forms a basis for evaluating the performance of various suboptimal algorithms, including the traditional algorithms mentioned in the introduction. Since model probability (13) depends on particular sample Yi , we consider its value averaged over the ensemble of samples: Pr i  H  hk  

(18)

where Pr (H  hk / Yi ) is the probability of hypothesis l i

l

H  hk calculated in accordance with (13) for sample Yi l . If

(20)

0; arg max f H  H / Yi l   htrue hk  . (21) ID   l 1; arg max f H  H / Yi   htrue hk  Thus we can compare (19) and (20) as ‘real’ and ‘calculated’ covariances. l i

3. EXAMPLE OF IDENTIFICATION PROBLEM SOLUTION To clarify the algorithm described above, consider its application for a typical problem of identifying the error model of an angular rate sensor. Generally, the errors of sensors comprise three error components. Measurements (2) can be written as (IEEE 2005):

yi  x0  xi  vi ,

(22)

where yi is the error value at time ti  ti 1  t ; x0 is the zero-mean random variable, describing the random bias component of the sensor error; xi is the zero-mean random sequence characterizing the fluctuating error component such as in-run zero drift; vi is the zero-mean white noise describing the high-frequency error component. The model of in-run zero drift is commonly unknown and should be identified. Since there are two most widespread hypotheses for the drift model, we assume that K  2 in approximation (3). The first hypothesis is the random walk (Wiener process) drift model, the shaping filter of which is given by xi1  xi11  w w,

(23)

where wi1 is the Gaussian system white noise with the unit RMS; σ w is an unknown parameter. Then, after presenting the state vector in the form xik   xik   model (1), (2) are written

T

x0  , the matrices of  1 (θ1 )  I 2 x 2 , as

0 , H 1 (θ1 )  1 1 , 1i (θ1 )  v . Here I 2 x 2 T

1 Г 1 (θ )  w

l L

1   Pril (H  hk / Yi l )  , L l 1

1 l L IDil  ,   L l 1

where IDil can be defined as:

i

T 1 l L k  (l )  ˆ k (Yi l ) k (l )  ˆ k (Yi l ) ,  L l 1

(19)

is called a ‘mean calculated probability of false identification’. Using the Monte Carlo method, we can also calculate the actual (real) probability of false identification of algorithm (5) for each time step as the ratio of incorrectly determined samples to the total number:

by using the Monte-Carlo methods as: Gik  GiRk 

1 l L  1  Pril (H  hk / Yi l )  L l 1

is a 2  2 unit matrix. The vector of unknown parameters θ1  w

v 

T

includes parameter σ w and the unknown

RMS of measurement white noise v . The second hypothesis

2887

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 O.A. Stepanov et al. / IFAC PapersOnLine 50-1 (2017) 2830–2835

where wi2 is the Gaussian system white noise with the unit

0

200

0.5 0.4 0.3 0.2

400

0

200

t [s]

T

400 t [s]

Средняя ошибка определения гипотезы

Fig. 0.062. Parameter estimation for the 100 exponentially correlated Марковской process. Винеровской w

2 2/Hz] Спектральная плотность [ед /Гц]  СКО оценки PSD [unit

v  0.5 2 o / s ,

matrices (16), (17), the number of samples for each hypothesis was taken to be L  500 . The results demonstrate that the mean calculated probability of false identification (19) is less than 0.1, i.e., 10% after T  1000 s (Fig. 1). As is seen, it corresponds to the actual probability of false identification (20).

0.04

100.02 1

0 0 10

200

400

(a) 10

-2

-1

10 Frequency Частота [Гц][Hz]

60

2

10

0

estimated true

40 0

600

estimatedt [с] true

0

10

80

10

200

400

600

t [с]

0

(b) 10

-1

10

0

Частота [Гц][Hz] Frequency

Fig. 3. Estimated PSD for the exponentially correlated process (a) and random walk (b) mixed with white noise.

Средняя вероятность ошибки идентификации

calculated Расчетная Действительная actual

40 30

estimated true

-0.1

10

estimated true

0

10

СКО Аллана [ед]

Allan СКОdeviation Аллана [ед][unit]

Probability [%][%] Погрешность

0.02

v  .

400

0.6

0.025

0.015

  0.01 0.1 1/ s , m  0.5 2 / s . To calculate the

50

200 t [s]

0.03

o

60

0 0

400 t [s]

The problem was simulated with the assumptions that the hypotheses for the random walk and exponentially correlated processes are equiprobable, and the components of vectors θ1 , θ 2 are distributed uniformly over the intervals w  0.01 0.2 o / s 2 ,

0.2

Ошибка [%]

i2 (θ2 )  v , and the parameter vector θ2   m

0.4

m RMS [deg/s]

H 2 (θ2 )  1 1 ,

200

[ед/с]

 2 / t (1  exp(t ))  Г 2 (θ 2 )   m , 0 

0.6 0.4 0

 RMS [1/s]

RMS;  m is the RMS error of the process,  is the parameter inverse of the correlation interval, t is the sampling interval. exp(t ) 0 The model matrices are given by  2 (θ 2 )   , 0 1  

v RMS [deg/s]

(24)

calculated real

0.8

2 Спектральная плотность [ед /Гц]

xi2  exp(t ) xi21  m 2 / t (1  exp(t ))wi2 ,

х RMS [deg/s]

is an exponentially correlated drift model hypothesis with the shaping filter

2833

-0.1

10

-0.2

10

-0.3

10

(b)

(a)

20

1

10 Время осреднения [c] Averaging time (s)

0

10

1

10

Время осреднения [c] Averaging time (s)

2

10

10 0 0

100

200

300 Время [s] [с] Time

400

500

600

Fig. 1. Mean probability of false identification [%]. RMS errors characterizing the potential accuracies (16), (17) for the vector θ2   m

v 

T

corresponding to the

second hypothesis are given in Fig. 2. As follows from Fig. 2, the calculated and real RMS estimation errors coincide, which confirms the validity of the results obtained. It should be noted that traditional algorithms such as estimation of PSD, correlation function, and Allan variance generally fail to provide the desired results. Figure 3 presents the estimated PSD for two model implementations for the first and second hypotheses, and Fig. 4 presents Allan variances. As is seen, in this case it is rather difficult to distinguish between the processes relying on these estimated characteristics

Fig. 4. Allan variances for the exponentially correlated process (a) and random walk (b) mixed with white noise. Also, additional algorithms are required to estimate the model parameters. The parameter estimation accuracy of the proposed algorithm is sometimes 2–5 times higher than that of the traditional algorithms and Allan variance (Stepanov and Motorin 2014, Tupysev et.al. 2016). The length of the sample used to solve the identification problem can be also reduced. 4. APPLICATION RESULTS To date, certain experience has been gained in using the proposed algorithms for identification of error models of various sensors. 4.1 Identification of sensor errors As an example, consider identification of the error model for ADIS 16405 accelerometers (Analog.com, 2016). The model was assumed to be presented in form (22) with the same hypotheses for the bias drift as those in the simulation example (23), (24). The estimate was calculated for four independent measurement samples. From Fig. 5 we can

2888

Proceedings of the 20th IFAC World Congress 2834 O.A. Stepanov et al. / IFAC PapersOnLine 50-1 (2017) 2830–2835 Toulouse, France, July 9-14, 2017

0.5

1000

2000

6

Sample number

0.022 0.02 0

1 2 3 4

x 10

(c)

2

4 1 2 3 4

500 1000 1500 2000 2500 3000

Time [s]

СКО 1 2 3 4

2000

3000

Time [s]СКО Расчетное

(d)

Computed RMS 2

0 0

5 0 0

500 1000 1500 2000 2500 -4 Время [c] Time [s]

x 10 1 2 3 4

2.5 2 1.5

(d)

Computed RMS

1

1 2 3 4

0.5 500 1000 1500 2000 2500 Время[s][c] Time

0

500 1000 1500 2000 2500 Время [c] Time [s]

4.2 Identification of map errors 1000

-4

x 10

Computed RMS

3000

10

1 2 3 4

Fig. 7. Estimation of the error model parameters for ADIS 16405 accelerometers using Allan variance: white noise PSD (a), the system noise PSD of the random walk (b), and relevant computed RMS errors (c), (d).

(b) 0 0

5

0

2

Time [s] СКО Расчетное

Computed RMS

System noise PSD  3   m/s / Hz 

4

500 1000 1500 2000 2500 -3

0

x 10

(a)

0.024

4

-4

8

1000 2000 Время [c] Time [s]

0

Figure 6 presents all four estimates of parameters for all samples and the computed RMS errors (14) of these estimates for the random walk hypothesis. 2 White noise PSD  m/s / Hz  

1 2 3 4

(c)

3000

Time [s]

0.026

0.01

(a)

Fig. 5. Computed probabilities of hypotheses.

0.028

15

0.02

0 0 -5 x 10 10

0 0

System noise PSD 3 (b)  m/s / Hz   -4

x 10

Оценка

Exp. correlated Марковская гипотеза Random walk гипотеза Винеровская

White noise PSD  m/s 2 / Hz  

СКО

1

0.03

Оценка

Hypothesis probability

clearly see that for all samples the random walk hypothesis has much higher probability.

1000

2000

1 2 3 4

3000

Time [s]

Fig. 6. Estimation of the error model parameters for ADIS 16405 accelerometers: white noise PSD (a) and the PSD for the system noise of the random walk (b), and relevant computed RMS errors (c), (d). As seen from the figure above, all estimated PSDs of white noise converge to approximately the same value ˆ  0.025 (m/s2 ) / Hz , and the calculated RMS error is

The developed algorithms were also applied to identify the models of total errors of digital maps of geophysical fields and sensors – gravity anomaly and depth field. Such models are needed to solve the filtering problem for map aided navigation (Nebylov and Watson, 2016). The corresponding error samples can be obtained as in (Stepanov et al. 2014). From the data of the preliminary survey of the geophysical field, which is a set of trajectories on which the geophysical parameter was measured, we exclude one of the trajectories and use the others to construct a digital map. This map is used to calculate the values of the geophysical field at the points corresponding to the measurements on the excluded trajectory and in this way a set of values taken from the map is formed. A set of differences between the measured and calculated values of the field is formed as

  5 106 (m/s2 ) / Hz . As to the system noise of the

1l  glapp1  gl1 , l  1.m1g ,

random walk, its PSD is qˆ  2. 104 (m/s3 ) / Hz , and the

where g l1 , glapp 1 , l  1.m1g is a set of measured field values and those calculated using the map at the points with known coordinates. This procedure is repeated, the difference being in that the next trajectory is excluded and thus the samples of errors for the rest of the trajectories are obtained. Thus, the total error samples with 100 measurements in each sample were obtained for four different map areas. For the model identification we used the problem statement with two hypotheses similar to that in the example considered above. The results for the gravity anomaly field have shown that for 100% samples from the first area, 91% from the second area, 78% from the third area, and 96% from the fourth area, the model proved to be the most probable, with the mean calculated probability of correct identification (18) about 99% in all the cases. Moreover, it should be mentioned that the more paths were used to construct a map in a certain area, the lower was the RMS of the noise and that of the correlated components, and the larger was the error correlation interval. The distribution of parameter estimates corresponding to the hypothesis of the exponentially correlated process is presented in Fig. 8.

RMS error is q  6 105 (m/s3 ) / Hz . Note that the results are consistent in the sense that the accelerometers have nearly similar error models. These data were processed using Allan variance and the parameter estimation algorithms (Stepanov and Motorin 2014, Tupysev et al. 2016). The results are presented in Fig. 7. One can see that at the end of the time interval, the estimates converge to the same values as in the proposed algorithm. But the settling time is shorter for the nonlinear algorithm, especially for the white noise PSD estimates; moreover, during this time, its estimates are much smoother than those obtained with Allan variance. The Allan variance RMS errors seem smaller than those from the proposed approach, especially in the early time steps, but they are not consistent with a real error, which is confirmed by a large scatter of estimates and the simulation done in (Stepanov and Motorin 2014, Stepanov et al. 2015).

2889

(25)

Proceedings of the 20th IFAC World Congress Toulouse, France, July 9-14, 2017 O.A. Stepanov et al. / IFAC PapersOnLine 50-1 (2017) 2830–2835

0.012

White noise СКО шумаRMS [мГал][mGal]

Process RMS [mGal] СКО процесса [мГал]

2 1.5 1 0.5 0 0

Участок Area 11 Участок Area 22 Участок Area 3 3 Участок Area 44 50

100

150

Интервал корреляции Correlation interval [мин] [min]

200

0.01 0.008 0.006 0.004 0.002 0 0

Area 11 Участок Area 22 Участок Участок Area 33 Участок Area 44 50

100

150

Интервал корреляции [мин] Correlation interval [min]

Fig. 8. Distribution of error estimation parameters over various survey areas. From Fig. 8, particularly, it follows that the RMS error of the white noise component is negligibly small as compared with the total RMS error. Similar computations were made for the depth field (Tupysev et al. 2016), for which the hypothesis of the exponentially correlated error was confirmed with higher probability. The white noise component was also higher. It should be emphasized that the traditional methods could not be used to identify the map errors because it was impossible to obtain measurements of required lengths. 5. CONCLUSIONS The problem-oriented approach to identification of the structure and parameters of error models of navigation sensors has been suggested. Its major advantages are the following: the possibility of obtaining a model in the form that is convenient for solution of estimation problems in integrated navigation systems using stochastic Bayesian filtering algorithms; possibility of estimating an unknown structure and parameters of a model at any time interval both for stationary and nonstationary sequences and processes; and capability of calculating characteristics of estimation accuracy. The efficiency of the proposed approach for identification of models and their parameters for typical processes describing errors of navigation sensors are analyzed. The suggested algorithm is compared with the existing methods based on estimation of correlation functions, power spectral densities, and Allan variance. Some examples of application of the proposed approach are provided, which demonstrate that the obtained algorithms outperform the existing ones. REFERENCES Allan, D.W., (2015) Historicity, strengths, and weaknesses of Allan variances and their general applications, Gyroscopy & Navigation, 2016, vol. 7, No. 1, pp. 1-17. Analog.com. (2016). ADIS16405 Datasheet and Product Info Analog Devices. [online] Available at: http://www.analog.com/en/products/sensors/inertialmeasurement-units/adis16405.html#product-overview [Accessed 6 Nov. 2016]. Bar-Shalom, Y., Willet, P., Tian, X., (2011) Tracking and data fusion: A handbook of algorithms, YBS Publishing, Storrs, CT. Beloglazov, I.N. and Kazarin, S.N., (1998) Joint optimal estimation, identification, and hypothesis testing in discrete dynamic systems, J. Comp. and sys. sciences international, vol. 37, no. 4, pp. 534–550.

2835

Bendat, J.S., and Piersol, A.G., (2010) Random Data: Analysis and Measurement Procedures, 4th Edition, Wiley, New York. Dmitriev, S.P. and Stepanov, O.A., (2001) Solving multiple model navigation problems by using nonlinear filtering, Proc. 5th IFAC symposium on nonlinear control system, pp. 685–690. Doucet, A., de Freitas N., and Gordon N., (2001) Sequential 200 Monte Carlo methods in practice, Springer-Verlag, NewYork. Gibbs, B.P. (2011). Advanced Kalman Filtering, LeastSquares and Modeling. A Practical Handbook, John Wiley&Sons, Inc. IEEE (2005) 1554-2005, IEEE Recommended Practice for Inertial Sensor Test Equipment, Instrumentation, Data Acquisition, and Analysis. IEEE Aerospace and Electronic Systems Society. Krobka, N.I., (2015) On the topology of the Allan variance graphs and typical misconceptions in the interpretation of the structure of the gyro noise, Proceedings of 22nd St. Petersburg International Conference on Integrated Navigation Systems, pp. 525–550. Lainiotis, D.G., (1976) Partitioning: a unifying framework for adaptive systems, I: Estimation, IEEE Trans., vol. 64, no. 8, pp. 1126–1140. Magill, D.T., (1965) Optimal adaptive estimation of sampled stochastic processes, IEEE Trans. Autom. Control, vol. AC-10, pp. 434–439. Nebylov, A. and Watson, J., eds., (2016) Aerospace Navigation Systems, Chichester, UK: John Wiley & Sons Ltd. Sarkka, S., (2013) Bayesian filtering and smoothing, in IMS Textbooks series, vol. 3, University Press, UK: Cambridge. Stepanov, O.A., Sokolov A.V., Toropov, A.B., Vasil’ev, V.A., and Krasnov, A.A., (2014) Choosing informative trajectories in the problem of correlation extreme navigation with consideration for errors of map and sensors, Materialy 29 konferentsii pamyati vydayushchegosya konstruktora giroskopicheskikh priborov N.N. Ostryakova (Proc. 29th Conf. in Memory of N.N. Ostryakov), pp. 217–225. Stepanov, O.A., Motorin, A.V. (2014) Identification of sensor errors: Allan variance vs nonlinear filtering. The 21st Saint Petersburg International Conference on Integrated Navigation Systems, pp. 123-128. Stepanov, O.A., Motorin, A.V., and Vasiliev, V.A., (2015), Identification of sensor errors by using nonlinear filtering, IFAC-PapersOnLine, vol. 48, iss. 11, Proc. of 1st conference on modeling, identification and control of nonlinear systems, MICNON-2015, pp. 808-813 Stepanov, О.А., (2016). Optimal and sub-optimal filtering in integrated navigation systems. In: A. Nebylov, J. Watson eds. Aerospace Navigation Systems, Chichester, UK: John Wiley & Sons Ltd., pp. 244–298 Tupysev, V.A., Motorin, A.V., and Kruglova, N.D., (2016) Suboptimal algorithms for identification of navigation sensor errors described by Markov process, Giroskopiya i Navigatsiya, no. 3, pp. 55–62.

2890