AIAA GNC, 11 Aug. 2009

Mark L. Psiaki, Sibley School of Mechanical & Aerospace EngineeringCornell University

Kalman Filtering & Smoothing to Estimate Real-Valued States & Integer Constants

GNC/Aug. ‘09 2 of 21

Goal Improve estimation algorithms for systems that have

integer measurement ambiguities CDGPS with double-differenced integer ambiguities Systems using carrier-phase measurements of TDMA

signals

Use SRIF/LAMBDA-type formulation to deal with mixed real/integer problem

Develop optimal & suboptimal Kalman filter & smoother algorithms Optimal: keep all ambiguities & treat as integers Suboptimal: retain integers in a finite time window

Strategies

GNC/Aug. ‘09 3 of 21

Outline of TalkI. Related researchII. Problem definitionIII. Mixed real/integer Kalman filter

Optimal, retains all past integers Suboptimal, retains finite window of past integers

IV. Mixed real/integer fixed-interval smoother Optimal, retains all integers of fixed interval Suboptimal, retains finite window of past & future integers

relative to each time point

V. Truth-model simulation & resultsVI. Conclusions

GNC/Aug. ‘09 4 of 21

Related Research: Batch estimation w/integer ambiguities

The LAMBDA method, Teunissen (1995) & follow-ons Other methods, e.g., Chen & Lachapelle (1995) SRIF LAMBDA-like method, Psiaki & Mohiuddin (2007)

Kalman filtering w/integer ambiguities Standard Covariance EKF, Kroes et al. (2005) SRIF-based EKF, Mohiuddin & Psiaki (2008) Sub-optimal dropping of each integer ambiguity immediately

after its last occurrence in a measurement Smoothing w/integer ambiguities

Nothing

GNC/Aug. ‘09 5 of 21

Dynamics ModelReal-state dynamics:

Partitioning of integer states by affected measurement sample times (past, past & present, past, present & future):

kkkkkk ΓΦ wxx 1

kk

k nnn 1

Growth of integer state with sample number

kk

kk

ΠΔ

Δ

n

nn

n

1 Or dynamic re-partitioning

kk

kkk Π~ n

nnn

11

GNC/Aug. ‘09 6 of 21

Measurement Model

111

11~

ykkk

nkkwkkxkk HHH nnwxy

… using integer vector partitions

… using full integer vector

1111 ykknkkwkkxkk HHH nwxy

111 000000~

knknk ΠI

IHH

GNC/Aug. ‘09 7 of 21

Example Sensitivities of Different Measurement Types to Different Integers

0 1000 2000 3000 4000 5000 6000 7000 8000 90001

2

3

4

5

6

7

8

9

10

11x 10

-3

Time (sec)

Am

bigu

ity S

ensi

tiviti

es, h

tilde

(m)

GNC/Aug. ‘09 8 of 21

Kalman Filtering/Smoothing Problem find: x0, …, xk+1, w0, …, wk, & nk+1 = [n0; …; nk] to minimize:

subject to: xj+1 = jxj + jwj + j for j = 0, 1, 2, ..., k nk+1 is an integer-valued vector

][][ 000T

00021

xxxxxx ˆRˆRJ zxzx

k

jjwwjjwwj RR

0

T21 ][][ ww

k

jj

jnjjwjjxj HHH

0

T1

012

1 ][{ yn

nwx

}][ 10

1

j

jnjjwjjxj HHH y

n

nwx

GNC/Aug. ‘09 9 of 21

Stage-k a posterior info:

Combined information eqs. w/dynamics substitution for xk:

New stage-(k+1) a posterior info after QR factorization:

Optimal SRIF Kalman Filterxkxkkxnkkxxk RR ˆˆˆ znx

nknkknnkR ˆˆ zn

111

1

1

1

111

11

ˆˆ

ˆˆˆ

0

]0,ˆ[00]0,ˆ[ˆˆ

00

yknkxkwk

kkxkk

nkkkxxkxk

k

kk

nkkxkkkxkwk

nnk

xnkkxxkkkxxk

wwk

ΦH

ΦR

HΦHΓΦHHRRΦRΓΦR

R

yz

z

nxw

1

1

1

1

1

1

1

11

11

11

ˆˆˆ

ˆˆˆ

000

ˆ00

ˆˆ0

ˆˆˆ

rk

nk

xkwk

rk

nk

xkwk

k

kk

nnk

xnkxxk

wnkwxkwwk

RRRRRR

zzzz

nxw

GNC/Aug. ‘09 10 of 21

Measurement Update via Integer Linear Least-Squares Solution Solve integer linear least-squares problem to

determine integer a posteriori estimate

Back-substitute to compute real-valued states:

]ˆˆ[]ˆˆ[ 111T

11121

nkknnknkknnk RR znzn1

ˆkn

]ˆˆˆ[ˆˆ 1111

11

kxnkxkxxkk RR nzx

)(min 1kJ n

GNC/Aug. ‘09 11 of 21

Suboptimal KF Retention of Exact Integers within a Window of Samples

0 1000 2000 3000 4000 5000 6000 7000 8000 90001

2

3

4

5

6

7

8

9

10

11x 10-3

Time (sec)

Am

bigu

ity S

ensi

tiviti

es, h

tilde

(m)

Measurements used in tk = 3000 secsub-optimal filter

tk = 3000 sec +0/- i*deltat windowfor considering exact integers

kk

i,k

k Δ

Δ

nn

n

m )1(max

GNC/Aug. ‘09 12 of 21

Stage-k a posterior info:

Combined information eqs. w/dynamics substitution for xk & mk

New stage-(k+1) a posterior info after QR factorization:

Suboptimal SRIF Kalman Filter xkxkkxmkkxxk RR ˆˆˆ zmx

mkmkkmmkR ˆˆ zm

111

1

1

1

111

11

ˆˆ

ˆˆˆ

0

0

ˆ00ˆˆˆˆˆ

000

ykmkxkwk

kkxkk

mkkkxxkxk

k

kkik

mkkxkkkxkwk

mmbkmmak

xmbkkxxkkkxxkxmak

wwk

ΦH

ΦRΔ

HΦHΓΦHHRRRΦRΓΦRR

R

yz

z

mxwn

1

1

1

1

1

1

1

1

1

11

11

11

ˆˆˆˆ

ˆˆˆˆ

0000

ˆ000

ˆˆ00

ˆˆˆ0

ˆˆˆˆ

rk

mk

xkwkΔnk

rk

mk

xkwkΔnk

k

kkik

mmk

xmkxxk

wmkwxkwwk

ΔnmkΔnxkΔnwkΔnΔnkΔ

RRRRRRRRRR

zzzzz

mxwn

GNC/Aug. ‘09 13 of 21

Terminal sample K initialization:

1-sample backwards recursion starts w/filtered wk & smoothed xk+1 info. eqs. & uses dynamics to get

QR factorize to isolate smoothed xk info. eq.

Optimal RTS Smoother in SRIF Form

,ˆ K*K nn ,RR xxK

*xxK KxnKxK

*xK ˆRˆ nzz K

*K xx i.e.,

*xk

wk

k*xxk

*xk

*kwnkkwxkwk

kk

k*xxkk

*xxk

kwxkkwxkwwk ˆ

R

RRˆ

RR

RRR

111

111

11

11

z

nzxw

*xk

*wk

*xk

*wk

kk

*xxk

*wxk

*wwk

R

RR

zz

xw

0 )]0 [

known(with

1*k

*k I nn

GNC/Aug. ‘09 14 of 21

Suboptimal RTS Smoother Retention of Exact Integers within a Window of Samples

0 1000 2000 3000 4000 5000 6000 7000 8000 90001

2

3

4

5

6

7

8

9

10

11 x 10-3

Time (sec)

Am

bigu

ity S

ensi

tiviti

es, h

tilde

(m)

Additional measurements used intk = 3000 sec sub-optimal smoother

Measurements used in tk = 3000 secsub-optimal filter & smoother

tk = 3000 sec +/- i*deltat rangefor considering exact integers

)11( ik,Kmin

kk

k

n

nm

l

GNC/Aug. ‘09 15 of 21

Terminal sample K initialization:

1-sample backwards recursion starts w/filtered wk & nk-i & smoothed xk+1 & lk+1 info. eqs. & uses dynamics & integer permutation/partitions to get

Suboptimal RTS Smoother (1 of 2)

,RR xxK*xxK

,ˆ xK*xK zz

,RR xmK*xlK ,RR mmK

*llK

mK*lK zz

*lk

*xk

wkΔnk

*lk

k*xxk

*xk

kwxkwk

kΔnxkΔnk

kkkik

*llak

*llbk

*xlakk

*xxkk

*xxk

*xlbk

wlkkwxkkwxkwwk

ΔnlkkΔnxkkΔnxkΔnwkˆˆ

R

RˆRˆ

RR

RRΓRR

RRΓRRRRΓRR

1

1

1

11

1

1

11

11

11

00

00

z

z

zz

lxwn

GNC/Aug. ‘09 16 of 21

Suboptimal RTS Smoother (2 of 2) New stage-k smoothed xk & lk square-root information

equations after QR factorization

is the integer vector that minimizes

The real part of the state is determined by back substitution:

*lk

*xk

*wk

*nk

*lk

*xk

*wk

*nk

kkkik

*llk

*xlk

*xxk

*wlk

*wxk

*wwk

*nlk

*nxk

*nwk

*nkn

RRRRRRRRRR

zzzz

lxwn

00000

0

*kl

][][)( T *lkk

*llk

*lkk

*llkk RRJ zlzll

)()( 1 *k

*xlk

*xk

*xxk

*k RR lzx

GNC/Aug. ‘09 17 of 21

Example 1-Dimensional CDGPS-Type Problem with 3rd-Order Dynamics Dynamics:

Measurements:

kkk

k

kkkkk

k ΔtΔtΔt

qΔtΔtΔtΔt

wxx

01/331/3/500)34/(4/5000)52/(

10010

0.5122

1

k

kk

kk

s

s

kΔtΔt

ΔtΔt

kkr

kxy

2

2

10.1255.01

0.1255.01

)/1(000000)/1( {1

1

3205

1921

1283

1920531

3205

1921

1283

1920531

2 }11

000000

yks

s

s,k

s,k

kkk

kkr

k

kkr

k

n

n

h~

h~

qΔtΔt w

GNC/Aug. ‘09 18 of 21

x1 Errors for Three Kalman Filters

0 1000 2000 3000 4000 5000 6000 7000 8000 9000

-0.1

-0.05

0

0.05

0.1

Time (sec)

x 1 filte

r erro

r (m

)

OptimalSuboptimal, i = 40Suboptimal, no integers

GNC/Aug. ‘09 19 of 21

x1 Errors for Three Smoothers

0 1000 2000 3000 4000 5000 6000 7000 8000 9000-0.015

-0.01

-0.005

0

0.005

0.01

0.015

Time (sec)

x 1 sm

ooth

er e

rror (

m)

OptimalSuboptimal, i = 40Suboptimal, no integers

GNC/Aug. ‘09 20 of 21

Integer-Part Computational Cost of Optimal & Suboptimal Algorithms

0 1000 2000 3000 4000 5000 6000 7000 8000 90000

0.5

1

1.5

2

2.5

3

3.5

4 x 104

Time (sec)

Cum

ulat

ive

L tot IL

LS E

xecu

tion

Cos

t Met

ric

Optimal KFSuboptimal KF, i = 40Suboptimal KF & Smoother, i = 40

GNC/Aug. ‘09 21 of 21

Summary & Conclusions Developed optimal & suboptimal Kalman filters & fixed-

interval smoothers for mixed real/integer estimation problems Constant integer ambiguities enter only measurements Optimal algorithms consider all integers in data batch Suboptimal algorithms drop integers that affect measurements

only in remote past or future Tested using data from truth-model simulation

Optimal & suboptimal filter achieve modest accuracy gains vs. all-reals approximate filter

Filter accuracy gains may be greater for different problem Optimal & suboptimal smoother significantly more accurate

than all-reals smoother Suboptimal smoother nearly as accurate as optimal smoother Suboptimal algorithms reduce required processing by at least

65% through reductions in dimensions of measurement update integer linear least-squares problems