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 research

II. Problem definition

III. 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 & results

VI. 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

k

kk n

nn 1

Growth of integer state with sample number

kk

k

kΠΔ

Δ

n

nn

n

1 Or dynamic re-partitioning

k

kk

k

k Π~

nn

nn

1

1

GNC/Aug. ‘09 6 of 21

Measurement Model

11

111

~

ykk

knkkwkkxkk HHH

nn

wxy

… 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

big

uity

Se

nsi

tiviti

es,

htil

de

(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

0

121 ][{ y

n

nwx

}][ 1

0

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

yk

nk

xk

wk

kkxkk

nk

kkxxkxk

k

k

k

nkkxkkkxkwk

nnk

xnkkxxkkkxxk

wwk

ΦH

ΦR

HΦHΓΦHH

R

RΦRΓΦR

R

y

zz

nxw

1

1

1

1

1

1

1

11

11

11

ˆˆˆ

ˆˆˆ

000

ˆ00

ˆˆ0

ˆˆˆ

rk

nk

xk

wk

rk

nk

xk

wk

k

k

k

nnk

xnkxxk

wnkwxkwwk

R

RR

RRR

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 znzn

1ˆ

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

big

uity

Se

nsi

tiviti

es,

htil

de

(m

)

Measurements used in tk = 3000 sec

sub-optimal filter

tk = 3000 sec +0/- i*deltat window

for considering exact integers

k

k

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

yk

mk

xk

wk

kkxkk

mk

kkxxkxk

k

k

k

ik

mkkxkkkxkwk

mmbkmmak

xmbkkxxkkkxxkxmak

wwk

ΦH

ΦRΔ

HΦHΓΦHH

RR

RΦRΓΦRR

R

y

zz

mxwn

1

1

1

1

1

1

1

1

1

11

11

11

ˆˆˆˆ

ˆˆˆˆ

0000

ˆ000

ˆˆ00

ˆˆˆ0

ˆˆˆˆ

rk

mk

xk

wk

Δnk

rk

mk

xk

wk

Δnk

k

k

k

ik

mmk

xmkxxk

wmkwxkwwk

ΔnmkΔnxkΔnwkΔnΔnkΔ

R

RR

RRR

RRRR

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

k

k

k*xxkk

*xxk

kwxkkwxkwwk ˆ

R

RRˆ

RR

RRR

111

111

11

11

z

nzxw

*xk

*wk

*xk

*wk

k

k*xxk

*wxk

*wwk

R

RR

z

zxw

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

11x 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 sec

sub-optimal filter & smoother

tk = 3000 sec +/- i*deltat range

for considering exact integers

)11( ik,Kmin

k

k

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

k

k

k

ik

*llak

*llbk

*xlakk

*xxkk

*xxk

*xlbk

wlkkwxkkwxkwwk

ΔnlkkΔnxkkΔnxkΔnwkˆˆ

R

Rˆ

Rˆ

RR

RRΓRR

RRΓRR

RRΓRR

1

1

1

11

1

1

11

11

11

00

0

0

z

z

z

z

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

k

k

k

ik

*llk

*xlk

*xxk

*wlk

*wxk

*wwk

*nlk

*nxk

*nwk

*nkn

R

RR

RRR

RRRR

z

z

z

z

lxwn

000

00

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

kkk

kk

k ΔtΔt

Δt

qΔtΔtΔtΔt

wxx

01/331/3/5

00)34/(4/5

000)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(0000

00)/1( {1

1

3205

1921

1283

1920531

3205

1921

1283

1920531

2 }11

0000

00

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 fi

lter

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 s

moo

ther

err

or (

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

4x 10

4

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