Synthesis of integrated one-stage tracking algorithm for GNSS and ins based attitude estimation Текст научной статьи по специальности «Физика»

УДК 519.213

Synthesis of Integrated One-stage Tracking Algorithm for GNSS and INS Based Attitude Estimation

Alexander I. Perov*

National Research University "MPEI" Krasnokazarmennaya, 14, Moscow, 111250

Russia

Received 10.06.2016, received in revised form 14.09.2016, accepted 20.11.2016 In this article, the theory of optimal filtering of information processes is used to synthesize optimal one-stage estimation algorithm for the object orientation angles. The algorithm uses signals of the satellite navigation and inertial systems. The block diagram of an integrated single-stage filtration system for object orientation angles is presented.

Keywords: satellite radio navigation system, attitude determination, tracking, optimal algorithm, synthesis, inertial-satellite system. DOI: 10.17516/1997-1397-2017-10-1-22-35.

Introduction

Satellite navigation systems are now widely used for a variety of technical problems, including extracting information from phase of the received signals [1,2]. One such application is to determine the object orientation angles from the satellite radio navigation system signals received at spaced-apart points [3,4]. The approach is based on the fact that the signals coming from the navigation satellite in two spaced-apart points have a phase shift that depends on the angle between the base line connecting the two receiving points and the direction of arrival of the navigation satellite signal. By measuring this angle and knowing the direction on the navigation satellite, it is possible to determine the angle of orientation of the base line. To implement this approach, the navigation receivers are placed at the receiving points, and they measure phases of received navigation signals. Further, the phase difference is formed which carries information about the angle between the base line and the direction of arrival of the navigation satellite signal. The main problem is that the measured phase difference differs from the true phase difference (that is proportional to the difference of arrival times of signal phase front at the receiving points) by an integer number of periods of high-frequency radiation. That is, there is an ambiguity of phase measurement. To resolve this problem various algorithms of phase measurement were proposed [5-18]. The described methodology for determining the orientation angles using signals of satellite navigation systems is based on the principle of two-stage processing of signals from navigation satellites [12]. Parameters of received radio signals (delay, phase, Doppler frequency shift) are estimated in the first stage. Coordinates, velocity and orientation angles of an object are estimated in the second stage. A one-stage algorithm for determining of object orientation angles using navigation satellites signals was proposed. In this algorithm, the intermediate stage for the formation of estimations of pseudorange and pseudophases is omitted. Estimations of the

*[email protected] © Siberian Federal University. All rights reserved

required parameters are formed by processing the outputs of the discriminators (time and phase). It allows one to eliminate the ambiguity of phase measurements using prolonged observation time. Inertial navigation systems are also widely used to determine the object orientation angles [20]. Therefore, it is of interest to construct an integrated system of determining the orientation angle of the object from the signals of satellite and inertial navigation systems. This issue is considered in this article.

1. Setting synthesis problem

Let us consider a geocentric coordinate system OXYZ rigidly connected with the earth (Fig. 1). The triangle ABC defines the reference plane. Coordinate system (CS) OXcYcZc is to be related to this plane. The center Oc of the coordinate system is located in the ABC plane. The OYc axis is along AB line, the OYc axis is located in the base plane, and the OZc axis completes the coordinate system.

Fig. 1. Geometric scheme of the problem

The orientation of ABC triangle in OXYZ CS is defined by the Euler angles a1,a2 and a3

i T

(roll, pitch and yaw, respectively) or a = |ai a2 a3| is the vector of rotation angles of CS OXcYcZc relative to CS OXYZ.

Let us assume that ABC triangle moves in the CS OXYZ so that the orientation angle vector changes with time, i.e. a (t). Radio signals from n navigation satellites are received at points A , B and C.

A strapdown inertial navigation system (SINS) is placed at point Oc. The SINS own coordinate system (OCS) is aligned with the OXcYcZc coordinate system. We deal with the problem of determining the angular orientation of an object so we consider only gyro SINS measurements

y,

(I + mSjk ) ^k + bg,k + Ug,k ,

(1)

where k is the number of measurements; is the true angular rate vector in OCS SINS; bg,k is gyroscopes zero displacement vector; mg,k is the matrix of axes misalignment and scale factors errors; ng,k is the vector of noise errors which is white Gaussian noise with variance matrix

Dng = Dng I.

Let us assume that mg,k, bg,k are elements of the Wiener process

bg,k = bg,k-1 + Zgbk-!, mg,k = mg,k-1 + Zgm,k-1- (2)

where £gb k-1, £gm,k-1 are three-dimensional vectors of independent white Gaussian noise with variance matrices Dg b and Dg,m, respectively.

One needs to process the received navigation signals and SINS measurements in order to synthesize the optimal filtration system for angle vector a.

Let us introduce the direction cosines of the i-th navigation satellite and identify them, for example, with respect to point Oc. We also assume that direction cosines are the same for all other points of triangle ABC. (this assumption is acceptable when the characteristic size of a triangle

is much less than the distance to the satellite) ¡jxi = i °c, Myi = ^ , ^zi = i °c

Ri Ri Ri

where x°c, y°c ,z°c are coordinates of point Oc in the CS OXYZ; R is the distance to the i-th

navigation satellite.

Let us introduce unit vectors MNSii = |^xi Myi Mzi\ , i = 1,n in the CS OXYZ and unit vectors lA°c , lB°c , lC°c that specify the direction points A, B and C with respect to CS

OcXcYcZc. T

Let us transform mNS,i = \^xi ¡iyi ^zi\ , i = 1,n unit vectors in CS OcXcYcZc into 1a°c , ecef, 1b°c , ecef , lc°c, ecef of CS OXYZ (ecef )

lA°c, ecef (a) = U lcef {a) lA°c , lB°c, ecef (a) = U fef (a) lB°c , ( )

lc°c,ecef (a) = Uecef (a) lc°c,

where Uecef is the coordinate transformation matrix from CS OcXcYcZc into CS OXYZ CS defined as

Uecef (a) =

cos(a3) — sin(a3) 0 sin(a3) cos(a3) 0 0 0 1

1 0 0

0 cos(a1) — sin(a1) 0 sin(a1) cos(a1)

cos(a2) 0 sin(a2) 0 1 0 || — sin(a2) 0 cos(a2)

Let us consider satellite radio signal with code division coming from i-th satellite to point Oc. We write

so,i (t) = Ahc,i (t — To,i) cos ((wo + uD,i) t + + po,i), (4)

where A is the amplitude; w is the signal carrier frequency ; wDii is the Doppler frequency shift due to the motion of point A (Oc ) of triangle ABC; t0,8 is the delay of the signal as it propagates from the navigation satellite to the receiver; p0ii is the initial phase of the signal received from the i-th navigation satellite; hcii (t) are functions of modulation with ranging code; -ddii is the navigation data, taking the value 0 or 1.

Relation (4) is valid for a some time T when parameters wD, i, t0i remain constant. The received signals are shifted in phase at points A, B and C with respect to similar signals received at point Oc by the phase angles

2-kLa°cMNs,ilA°c,ecef (a) 2nLB°cmNS,ilB°l!,ecef (a)

0A,i (a) = ------, 0B,i (a)

0c,i (a)

A ' A

2nLC°cMNs,ilC°c,ecef (a)

T , (5)

A

where LA°c ,LB°c ,LC°c is the distance between point Oc and points A, B and C, respectively. Consequently, we can write (for the same time interval T)

SA,i (t) = Ahc,i (t - TA,i) cos ((wo + uD,i) t + + ^Qi + ^A,i (a) +

SB,i (t) = Ahc,i (t - TB,i) cos ((wo + wD,i) t + n$d,i + Po,i + ^B,i (a) + wb t),

sci (t) = Ahci (t - tc,i) cos ((wo + wd,i) t + ndd,i + Po,i + ^c,i (a) + wc,^, it).

where wA,wB,^,i,wC^,i is the Doppler frequency offset of signals at points A, B and C,

respectively, due to rotation about point Oc of triangle ABC; ta,i , tb,i , tc,i are delays of the

signal envelopes from the i-th satellite at receiving points A, B and C.

Strictly speaking, delays TAii, TByi, TCii depend on the angle vector. However, this effect would not be taken into account because it has little influence on the accuracy of estimation of the orientation angles with the use of carrier phase measurements.

Total satellite radio signals are received at some intervals. For example, interval may be equal to the period of ranging code. Total signals can be represented as

n

SA (t) = J2 SA , i (t) = i=1

n

= E Ahc,i (t - TA,i) cos ((wo + wD,i) t + n$d,i + Po,i + ^A,i (a) + wA,^,it),

(6)

SB (t) = SB,i (t) = i=i

n

= Ahc,i (t - TB,i) cos ((wo + wD,i) t + ndd,i + po,i + ^B,i (a) + wb

i=1

n

sc (t) = E sc,i (t) =

i=i

n

= E Ahc,i (t - tc,i) cos ((wo + wD,i) t + ndd,i + po,i + ^C,i (a) + wc,^,i)-

i=i

Let us write the equation of observations at points A, B and C as yA (t) = sA (t) + nA (t),

yB (t) = sB (t) + nB (t), yC (t) = sC (t) + nC (t). where nA (t) ,nB (t) and nC (t) are independent

white Gaussian noises with equal bilateral spectral density No/2.

i t

Let us introduce the observation vectors y (t) = iyA(t) yB (t) yC(t) | for which we can write

i |T | |T

y (t) = s (t) + n (t), where s (t) = |sA (t) sB (t) sC (t)| , n (t) = (t) nB (t) nC (t)| is the Gaussian white noise vector with the matrix of bilateral spectral density No = INo/2.

Let us assume that receivers perform synchronous sampling of the input process in time, so that the processing system receives an implementation at discrete instants of time tkji: ykji = s (tk,, ^ + nk,i, where tki = kT + iTd; T = NTd is the time step of discrete processing in the tracking system loop; Td is the sampling period in an analog-to-digital converter; nk , i is the

vector of independent discrete white Gaussian noise with equal variances ^ = , and No is

the one-sided spectral density of the power of internal noise of the receiver.

2T,

d

-1-1-1-1-1—- t

tk-1 tk-1,1 t k —1,2 ••• tk-1,N-1 tk

Fig. 2. Schematic of time indexing

Let us note that SINS measurements (1) are held at time points tk. The rate of change of the orientation angles vector is defined by the following equations at time points tk

ak = ak-1 + TQk-1 (7)

Qk = Qk-1 + €a,k-^

where £a,k-1 is the vector of discrete white Gaussian noise with variance matrix D^. Using (3), we write the expression for the phase angle (5) in the form

, , 2nLAoc »Ns,ilAOc,ecef (a) 2nLAOc »Ns,i(U''C'f (a)lAOc )

0A,i (a) =-!-=----

0B,i (a) = 0c,i (a) =

л л

2-klbO' V-Ns,ilBOc,e ,cef (а) 2nLBO'ßNs,: ,ef (a)lBOc )

л л

2nLCO' ß'Ns,ilCOc,e ■cef (а) _ 2nLcO'ßNs,> ;(ur ef (a)lcO' )

(S)

Let us assume that lA°c, lB°c , lc°c are constant and MHc,i is a slowly varying function of time. Differentiating (8) with respect to time, we have

d,0A,i (а) dt

d0B,i (а) dt

d.0c,i (а)

= =

iB

= =

2nLA0c t (dUecef (а) Л Vns,

Л

dt X

Derivatives in the right-hand side of (9) are

dt

'dUeccef (а)

dt

(а),

lA0c ,

dt

BOc

■CO'

dUecef (а)

c (а) TTecef TTecef -dt- = Uc (а) ¿c/in — ¿ecef /inUc (а)

(9)

(10)

О — Oz "y

where ¿¿c/in = "z О "x is the matrix of angular velocities of CS XCYCZC

"y "x О

is the matrix of angular

0 -tt3 0 Q3 0 0 0 0 0

velocities of the Earth with respect to inertial space. Upon substituting (10) into (9), we obtain

to inertial space measured by gyroscopes; ¿¿ecef/i

2KLao'

= Л

2-kLbo,

= Л

2nLc0'

= Л

-vNs,i( (urf (а) ¿Jc/in — iecef/inU"eCef (а)) lAOc) , lmns,í( (uref (а) ¿j c/in — ¿¿ ecef/inUecef (а)) Ib o J , -ßTNS,i ( (UPeCef (а) ¿¿c/in — ¿¿ecef/inUeCef (а)) lC0^ •

2. Synthesis of optimal filtering algorithm for orientation angles

Let us introduce the state vector Xk =

ак

°а,к bg,k m g,k

for which one can write the matrix equation

F=

Xk — FXk -1 + G£ k-1 where

03 03 03

I3 TI3 03 03 I3 03 03

03 I3 03 03 , G — 03 I3 03

03 03 I3 03 03 03 I3

03 03 03 I3

»fc-1

£gb,k-1 £ gm,k-1

I3 is the 3-by-3 identity matrix, O3 is the 3-by-3 zero matrix, mg,k is the vector of unknown elements of matrix mgk (1).

Observations Y

k-1,N-1 k-1

{Уk-1,0,Уk-1,1,•••, Yk-1,N-1, y^,rpy,k-l) in the time interval [tk-1,tk-1,N-1] (see Figure 1) contain information about the state vector Xk-1 corresponding to tk-1 point in time. Therefore, if we are interested in the current estimation of the xk-1 state vector when processing observations Y^k = (Y°'N-1 y1,n-1 Yk-1,N-1

0 — ( ± 0 , Y1'JV l,..., Yk-r 1} then one needs to

consider a posteriori probability density p (Xk-1 [21,22]

Y

for which we can write the equation

pX

k1

Yk

Y

p Xk-1

Y

k-2,N-1

cp [Xk-1

/ p [Xk-2

J — cv

k-2,N-1

Y

k-1,N-1

P (Yk-1 \Xk-1 ) k-2,NP (Xk-1 \Xk-2) dXk-2

Using all available observations Y§ at tk-1<N-1, we form the state vector estimation Xk-1 corresponding to the state vector Xk-1 at tk-1,N-1.

Let us write the equations of optimal filtering of the vector Xk, using the Gaussian approximation [21]. Let us assume that delays rAyi, rBji, rC,i and Doppler frequency offsets i = l,n are known. Then we obtain

dFk

Xk-1 — Xk-1 + Dx , k-1 X k-1 — FX k-2

Dx,k-1 — FDx,k-2FT + GD?Gt, Dx\_ 1

(X k-1)

~6X

(11)

D x

1

dF,

,k 1

k (Xk-1)

d X

d X

(12)

where XXk-1 is the filtered process estimation; Xk-1 is the extrapolated estimation of the filtered process; DX,k-1 is the error variance matrix; IDx,k-1 is the extrapolation of the error variance matrix;

Fk (Xk-1)—ln p (Yk-1'N-1 \Xk-1)

N

— c

+£ 02 1

a2

1=1 2

^]vA,iSA,i (tk-1,1) + VB,iSB,i (tk-1,1) + yc,isc,i (tk-1,1)

2D x ''

2g

(yu,c,k-1 - (I + mg,k-1~) &c,k-1 - bg,k-1) {yu,c,k-1 - (I + mg,k-1)&c,k-1 - bg,k-0 ,

sA,i (tk-1,i),sB,i (tk-1,i),sc,i (tk-1,i) is the alarm function (6) at discrete points in time, tk-1j e

0

0

d

[tk-1,1,tk-1,N], NTd = T is the time interval of multiple periods of ranging code:

SA,i (tk-1,l) = Ahc,i (tk-1,l - TA,i;k-l) X X cos ((uo + uD,i;k-1) tk-1,l + n$d,i;k-1 + P0,i + ^A,i;k-1 (a— ) + uA,^,i,k-lh-1,l) ,

SB,i (tk-1,1) = Ahc,i (tk-1,1 — TB,i;k-1) x (13)

X cos ((uo + UD,i;k-1) tk-1,l + n$d,i;k-1 + W0,i;k-1 + ^B,i; k-1 (ak-1) + Ub,^,i,k-\tk-1,l) ,

sc,i (tk-1,l) = Ahc,i (tk-1,l — Tc,i;k-1) x X cos ((uo + UD,i;k-1) tk-1,l + n-&d,i;k-1 + yo,i + ^C,i;k-1 (ak-1) + Uc,^,i,k-1 tk-1,l).

Derivatives in (11), (12) are understood as row vectors [12]. In these signals parameters <^0,i,k-1 and $d,i,k-1 are not informative. Therefore, we average the likelihood function, according to these parameters:

k-1,N-1 k_ 1 \Xk-1

k-1,N-1

P [Yk-1 \Xk-1) =

n n

J ... J p (yj-^-1 |Xk-1, (fo,i,k-1$d,i,k-1, i d(fo,1,k-1...d(fo,n,k-1

n n

1

-n —n

" / —1 T ^[[Io (Ui (a)) + c1 exP{2Dr~ (y",c,k-1 — (I + mg,k-1) Mc,k-1 — bg ,k 1) > i=1 ^ ng

(14)

(16)

x (y^,c,k-1 — (I + mg,k-1) &c,k-1 — bg,k-1)) ,

where Io (x) is zero-order modified Bessel function of the first kind;

U2 (ak-1) = U2i (ak-1) + U2i (ak-1), (15)

A n

Uc i (ak-1) ^^"E [yA , i (tk-1, l) hc, i (t — TA, i;k-1) X

an l = 1

X cos (uotk-1,l + (uDi,k-1 + UA,^,i;k-1) (l — 1) Td + ^A,i;k-1 (ak-1)) +

+yB,i (tk-1,l) hc (t — Tb ,i;k — 1) X X cos (uotk-1,l + (uDi,k-1 + UB,^,i;k — 1) (l — 1) Td + ^B,i;k-1 (ak-1)) +

+yc,i (tk-1,l) hc (t — TC,i;k-1) X X cos (uotk-1,l + (uDi,k-1 + Uc^,i;k-1) (l — 1) Td + ^C,i;k-1 (ak-1))] ,

AN

Us i (ak) = —2 J2 [yA,i (tk-1,l) hc,i (t — TA,i;k-1) X

an l = 1

X sin (uotk-1,l + (uDi,k-1 + uA,^,i;k-1) (l — 1) Td + ^A,i;k-1 (ak-1)) +

+yB,i (tk-1,l) hc (t — Tb,i;k—1) X X sin (uotk-1,l + (uDi,k-1 + uB,^,i;k-1) (l — 1) Td + ^B,i;k-1 (ak-1)) +

+yc,i (tk-1,l) hc (t — Tc,i;k-1) X X sin (uotk-1,l + (uDi,k-1 + uc^,i;k-1) (l — 1) Td + ^c,i;k-1 (ak-1))] •

Let us write (16) in the following form

Uc,i (ak-1) =

= IAi,k cos (^A,i,k-1 (ak-1)) + lBi,k cos (^B,i,k-1 (ak-1)) + Ici,k cos (^c,i,k-1 (ak-1)) — — (QAi,k sin (^A,i,k-1 (ak-1)) + QBi,k sin (^B,i,k-1 (ak-1)) + Qci,k sin (^c,i,k-1 (ak-1))) ,

Us,i (ak-1) =

= QAi,k cos (^A,i,k-1 (ak-1)) + QBi,k cos (^B,i,k-1 (ak-1)) + Qci,k cos (^c,i,k-1 (ak-1)) + + (IAi,k sin (^A,i,k-1 (ak-1)) + IBi,k sin (^B,i,k-1 (ak-1)) + Ici,k sin (^c,i,k-1 (ak-1))),

where

IAi,k = A a 2 n N J2 VA,i (tk-l = i i,l) hc,i (t - -TA,i,k -i) cos (wotk- i,l + (wDi,k- i +wA,$,i,k- i)(l - 1) Td),

IBi,k = A a 2 n N 2 VB,i (tk-l=i i,l) hc,i (t - -TB,i,k -i) cos (wotk- i,l + (wDi,k- i + wB,$,i,k- i)(l -1) Td),

Ici,k = A a 2 n N J2 yc,i (tk-l = i -i,l) hc,i (t - -Tc,i,k -i) cos (wotk- i,l + (wDi,k- i +wc,$,i,k- i)(l -1) Td),

Q Ai,k = A a 2 n N J2 yA,i (tk-l = i -i,l) hc,i (t - TA,i, k-i) sin (wotk- -i,l + (wDi,k- -i +wA,$,i,k- -i)( l - 1) Td)

QBi,k = A a 2 n N J2 yB,i (tk-l = i -i,l) hc,i (t - TA,i, k-i) sin (wotk -i,l + (wDi,k -i +wA,$,i,k- -i)( l - 1) Td )

Qci,k = A al N J2 yc,i (tk l = i -i,l) hc,i (t - TA,i, k-i) sin (wotk- -i,l + (wDi,k- -i +wA,$,i,k- -i)( 1 - 1) Td)

(18)

describe multichannel correlators, in which it is necessary to use appropriate estimations wDik-i,wA^tik-i,wB^titk-i,wc^itk-i instead of implementations of the Doppler frequency wDi,k-i,wAi, k-i,wB,$i,k-i,wc,$,i,k-i- These estimations are formed in independent tracking loops such as those described in [12].

Note that the correlation integrals (18) must be calculated in a uniform time scale. Substituting (15) into (13) and performing the necessary transformations, we obtain:

Ui (ak-i) = i\i , k + Q2iAi , k + IB i , k + QBi, k + I'Ci , k + QCi ,k+

+2cos(0A , i;k-1 (ak-i) - 0B,i;k-1 («k-i)) (IAi, klBi, k + QAi,kQBi,k) + +2 Sin (0A,i;k-i (ak-i ) - 0B,i;k-i (ak-i )) (lAi,kQBi,k - Q Ai,klBi,k ) + +2 COS (0A,i ; k-i (ak-i) - 0c,i ; k-i (ak-i)) (lAi,k Ici,k + QAi,k Qci,k) + (19)

+2 sin (0A,i ; k-i (ak-i) - 0c,i ; k-i (ak-i)) (IAi,k Qci,k - Q Ai,k Ici,k ) + + 2 COS (0B,i;k-i (ak-i) - 0c,i;k-i (ak-i )) (lBi,k Icik + QBi,k Qci,k) + +2 sin (0B,i;k-i (ak-i) - 0c,i;k-i (ak-i)) (iBik Qci,k - QBi,k Ici,k) •

Consider the derivative

dFk (Xk-i) dX

dFk (Xk-i) dFk (Xk-i) dFk (Xk-i) dFk (Xk-i)

da

d ft

dbn

dm n

(20)

Let us introduce the vector 0 = \0Aii • • • 0A,n 0B}i • • • 0B,n 0ci • • • 0c,n\ and consider

dFk (Xk-i) = d_ da

da d

¿(InIo (Xi (ak-i)))

d_ d0

¿(InIo (Xi (ak-i)))

d0 da

d ~ ~ d (InIo (Ui (ak))) • • • ^-(InIo (Un (ak))) • • • ^-(InIo (Un (ak)))

d0A,i ''' d0B,i

Let us introduce the vector of phase differences

d0c,n

d0 da

(21)

d

~d0

¿(InIo (Xi (ak-i)))

i=i

Components of this vector can be represented in the form

d

d0A(B,c),-,

(InIo (Ui (ak-i))) =

Ii (Ui (ak-i)) dUi (ak-i)

Io (Ui (ak-i)) d0A(B,c),i '

The derivative (ak l) is obtained by differentiating (12) with respect to parameter

d0A(B,c),

0a(b,c)i- For instance, for 0Ai we have

dUi (ak-i)

1 dU2 (ak-i)

30A,i 2Ui (ak-i) 30A,i [- sin (0A,i ; k-l (ak-l) - 0B,i ; k-l (ak-l)) (lAi,klBi,k + Q Ai,k QBi,k ) +

Ui (ak-l)

+ cos (0A,i ; k-l (ak-l) - 0B,i ; k-l (ak-l)) (IAi,kQ Bi,k - QAi,klBi,k ) -- Sin (0A,i ; k-l (ak-l) - 0C,i ; k-l (ak-l)) (lAi,k Ici,k + QAi,kQci,k ) + + COS (0A,i; k-l (ak-l) - 0C,i; k-l (ak-l)) (IAi,k Qci,k - Q Ai,k Ici,k )]•

Taking into account (3), we write relations for phases 0Aii;k-l (ak-l), 0Bii■ k-l (ak-l) 0c,i; k-l (ak-l) (5) in the form

0A,i; k-l (ak-l) = 0B,i;k-l (ak-l) = 0c,i; k-l (ak-l ) =

2nLAOcVNS,iUcCef (ak-l) 1aQc

A

2nLBOc MNs,iUTef ( ak -l) lBQc,c

A

2nLcOc MNs,iUecef (ak- -l) lcQc,c

A

Let us represent vector 0 in the form 0 = \0A 0B 0c\ , where 0A(B,c) =

\ \T 30 30

\0a,l ■ ■ ■ 0A,n\ , and write derivative as a block matrix

da

da

da

da

da

where

d^A.l d^B.i d^o.i

d0A da 90 b da d0c da

da da da

da da da

Consider the derivative

Let us introduce the following matrix

(23)

M

M.n

Taking into account (23), we can obtain the expression

30 a 2kLaoc

d

M ^ da (Uc

da A

Similar expressions can be obtained for derivatives

(uecef (ak-l) 1aoc) ■

d0B 2nLBOc M d (U f )

dB = ~T"M ^ 3a{Uc f (ak-l) lBOc)

30c_

da

2nL

cO

d

M •d (U

(24)

da

(uccef (ak-l) 1co„) ■ (25)

1

c

A

Let us note that differentiation with respect to the orientation angles a in (24), (25) should be applied only to elements of the transformation matrix uece f (a), e.g.

dUlcef (a)

dal

dUeccef (a)

dan

dUecccf (a)

da3

cos (a3) sin (a3) 0

cos (a3) sin (a3) 0

- sin (a3) cos (a3) 0

- sin (a3) cos(a3) 0

- sin (a3) cos (a3) 0

- cos(a3) 0

— sin(a3) 0

00

cos(a2) 0 sin(a2) 0 1 0 — sin(a2) 0 cos(a2) — sin(a2) 0 cos(a2)

0

- cos (a2) cos (a2) 0

- sin («2 )

0 0 0 1 0

0

- sin(a2)

sin (a2) 0

cos (a2)

0

- sin (a- ) cos (a- ) 10

0

- cos (a- )

- sin (a- )

0

0 cos (a- ) - sin (a- ) 0 sin (a- ) cos (a- ) 1 0 0 0 cos (a- ) - sin (a- ) 0 sin (a- ) cos (a- )

Taking into account the obtained expressions, we write

d$ da

2n

X

Laoc M . da (ucccf (ak-l) lAOc

LBOcm . da (Ucccf (ak-l) lBOc LcOcm . dda (U cccf (ak-l) IcOc

Then rewrite (21) in the form

dFk (Xk-i) _ 2ndFk (ak-i)

da

X

2n

X

Laocm . da (ucccf (ak-l) lAOc

da

d

LBOcm . da (UCccf (ak-l) lBOc

Lcocm . da (UCccf (ak-l) lcOc

t

ud,^;k

(26)

Derivative dFk 1) in (26) describes the phase difference discriminator. Dynamic model

(7) assumes that the phase difference tracking system has second order astatism. This tracking system forms the derivative of the phase difference (i.e., frequency Q). Taking this into account, we can discard additional frequency discriminators (on Q) in the integrated system as it was

0.

done in [12], i.e., we assume that

dFk (Xk-0 dFk (Xk-0 , N , Derivatives -—-, -—-in (20) become

dFk (Xk-l) dQ

dbn

d in n

dFk (Xk-l) _ uT

1

dbg

d,b;k D-(y",c,k-l - (I + mn,k-l) &c,k-l - bn,k-l) T >

Dng

dFk(Xk-l) _ T 1 ( /T , ïr» U \T d , „ ,

-din- _ ud,m;k _ \y",c,k-l - (I+ mn,k-l) ^ck-l-bn,k-l) din" (mn,k-lilc,k-l) ■

n nn n (27)

Here, ud,b is the vector discriminator for zero offsets gyroscopes; ud,m is the vector discriminator for the vector scale factor. Substituting (26) and (27) into (11), we write the equation for

the state vector estimation

Here

X

k-1 :

XX k-1 +Dx

k1

LaocM • da (Ueccef (âk-i) 1aoc LBocm • da (UCcc/ (ak-1 ) lBOc Lcoc m • daiue^ (a k-1 ) lcoc

ld, b

XX k-1 +Dx,

k1

Ud,a ; k 0

ud,b ; k ud,m ; k

Ud,a ; k

2n

T

Laocm • da (ucccf (ak-1) lAOc LbocM • da ¡ucccf Oak-1) lBOc LcocM • da (ucccf Oak-1) lcoc

is the vector of discriminators for the orientation angles.

Ud,-0 ; k

Udrf; k

(28)

A generalized block diagram of the single-stage integrated filtration system for orientation angles of an object is shown in Figure 3.

Fig. 3. A generalized block diagram of the single-stage integrated filtration system for orientation angles of an object

0

u

3. Analysis of discriminators of phases difference of satellite signals received by spaced antenna

Phase difference discriminators (22) are non-linear devices. Therefore, stable operation of these discriminators defines largely the quality of the integrated filtration system. Let us calculate

the discriminatory characteristic (DC) of the phase difference discriminator. The discriminatory characteristic is commonly understood as the relation between the mean value of the process at the output of the discriminator and discriminate parameter a with the values of other parameters of the filtration system equal to the true values [23]: Ux j = M

Then the phase difference discriminator (DC) is described by the relation

df

Ut j (ÔX) = M * / , X sine I --k—2"

M

U't, j I ^

Cd . sine (j^) X

m=j

where cd is some constant;

sin ( s^j,k - s^m.k +

. k - . k,

m. k -

k

M is

the number of points at which navigation signals are received (here M=3).

The results of analytical calculation of the discriminator characteristics from the given above relations were compared with the results of simulation of discriminator (22) in the open mode of tracking system. Fig. 4 shows the analytical and experimental DC for the phase difference discriminator for various values of the number of points of reception of navigation signals. In addition, linear functions with derivatives equal to the analytical value of DC slopes are shown in Fig. 4. The results of analytical calculations are very close to the experimental results.

Fig. 4. Analytical and experimental discriminatory characteristics of the phase difference discriminator

2

k

Conclusion

The optimum one-stage filtering algorithm for orientation angles of an object processing satellite radio navigation signals and inertial navigation system measurements was considered in this article. The algorithm was constructed with the use of the theory of optimal filtering of information processes. Optimal integrated filtering equations were obtained. In addition, they were represented in the form of a servo system including the orientation angles discriminator, discriminator of gyroscopes zero displacements, discriminator of gyroscopes scale factors and

complex smoothing filter. The algorithm does not include separate tracking systems for the phases of the received navigation signals (or phase differences) and procedures to resolve the ambiguity of phase measurements. The block diagram of an integrated single-stage filtration system for object orientation angles is presented. Analytical formulas for the discriminatory characteristics of the phase difference discriminator were given. It was found that discriminatory characteristics have stable equilibrium point that ensures reliable tracking of the parameters of the object orientation angles.

References

[1] P.Misra, P.Enge, Global Positioning System, Measurements and Performance, Second edition, 2012.

[2] E.D.Kaplan, C.J.Hegarty, Understanding GPS principles and applications., Artech House Inc., Norwood, Massachusetts, 2006.

[3] E.C.Clark, Attitude Determination Using GPS., PhD Thesis, Stanford University, 1992.

[4] B.W.Parkinson, J.J.Spilker, Global positioning system: theory and applications., AIAA (1996),vol. 2, 519-538.

[5] A.A.Povalyaev, Sputnikovye navigatsionnye sistemy: vremya, chasy, izmerenie i opredelenie koordinat (Satellite navigation systems: time, clock, measuring formatsii and determination of coordinates), Radio Engineering, 2008 (in Russian).

[6] I.A.Lipkin, Sputnikovye navigatsionnye sistemy (Satellite navigation systems), The university book, 2001 (in Russian).

[7] A.D.Boriskin, A.V.Veytsel, V.A.Veytsel, M.I.Zhodzishsky, D.S.Milutin, The equipment of high-precision positioning signals of global navigation satellite systems receivers consumers of navigation information, MAI-Print, 2010 (in Russian).

[8] V.N.Kharisov, A.E.Perkov, Algorithms of filtering at phase measurements, Radiotehnika, 7(1997), 90-101 (in Russian).

[9] V.N.Kharisov, N.T.Bulavsky, Filtering relative coordinates in the SRNS GLO-NASS: an approach based on the signal time, Radio engineering, 7(1999) (in Russian).

[10] L.S.Rozov, N.V.Sobtsov, Zadacha fil'tratsii pri neodnoznachnyh izmereniyah (The problem of filtration under ambiguous measurements), Technology and Electronics, 25(1980), no. 9 (in Russian).

[11] K.V.Penzin, Algoritmy fil'tratsii pri pri fazovyh izmereniyah (Algorithms operational processing multiscale measurements maximum likelihood), Technology and Electronics, 25(1990), no. 1 (in Russian)

[12] A.I.Perov, V.N.Kharisov, GLONASS. The principles of construction and operation, Radio Engineering, 2010 (in Russian).

[13] Y.L.Fateev, D.D.Dmitriev, V.N.Tyapkin, I.N.Ishchuk, E.G.Kabulova, The phase ambiguity resolution by the exhaustion method in a single-base interferometer, ARPN Journal of Engineering and Applied Sciences, 10(2015), no 18, 8264--8270.

[14] Yu.L.Fateev, D.D.Dmitriev, V.N.Tyapkin, I.N.Kartsan, A.E.Goncharov, Phase methods for measuring the spatial orientation of objects using satellite navigation equipment, IOP Conference Series: Materials Science and Engineering, 94(2015), 012022.

[15] Yu.L.Fateev, D.D.Dmitriev, V.N.Tyapkin, N.S.Kremez, V.N.Bondarev, Phase ambiguity resolution in the GLONASS/GPS navigation equipment, equipped with antenna arrays, International Siberian Conference on Control and Communications (SIBCON), 2015.

[16] M.S.Hodgart, S.Purivigraipong, New approach to resolving instantaneous integer ambi-guity resolution for spacecraft attitude determination using GPS signals, Proceedings of IEEE position location and navigation symposium, PLANS'00, 2000, 132-139.

[17] D.Lin, L.K.Voon, N.Nagarajan, Real-time attitude determination for microsatellite by LAMBDA method combined with Kalman filtering., 22nd AIAA international communication satellite systems conference, Monterey, 2004, 8.

[18] S.Verhagen, P.J.G.Teunissen, New global navigation satellite system ambiguity resolution method compared to existing approaches, J Guidance, Control, and Dynamics, 29(2006), no. 4, 981-991.

[19] A.I.Perov, Tracking Algorithm for Estimating the Orientation Angles of the Object Based on the Signals of Satellite Radio Navigation System, American Journal of Applied CSiences, 12(2015), no. 12, 1000-1013.

[20] M.S.Grewal, L.R.Wiell, A.P.Andrews, Glibal Positioning Systems, inertial navigation and integration, New York, Wiley, 2001.

[21] V.I.Tikhonov, V.N.Kharisov, Statistical analysis and synthesis of radio engineering devices and systems, Radio and Communications, 2004 (in Russian).

[22] A.I.Perov, Methods and algorithms for optimal signal reception equipment in consumers of satellite radio navigation systems, Radio Engineering, 2012 (in Russian)

[23] S.V.Pervachev, Radioautomatics, Moscow, 1982.

Синтез комплексного одноэтапного алгоритма фильтрации углов ориентации объекта по сигналам спутниковой и инерциальной навигационных систем

Александр И. Перов

Национальный исследовательский университет "МЭИ" Красноказарменная, 14, Москва, 111250

Россия

В статье с использованием теории оптимальной фильтрации информационных процессов синтезирован оптимальный одноэтапный алгоритм оценивания углов ориентации объекта по сигналам спутниковой радионавигационной и инерциальной навигационной системы. Приведена структурная схема одноэтапной интегрированной системы фильтрации углов ориентации объекта.

Ключевые слова: спутниковая радионавигация, угловая ориентация, слежение, оптимальный алгоритм, синтез, инерциально-спутниковая система.