...
f(x0) . ( x- x0) f ' (x0) . (x - x0)
f( x ) = f (x0) + ------------ + ---------------------
1! 2!
...
d(EstDi) . delta ( x ) d(EstDi) . delta( y )
PDi = Est Di + ------------ + ------------------
dX dY
...
(Xest - Xli) (Yest - Yi)
ODi = Est Di + - ---------. delta ( x ) + ----------------- delta ( y) + c. delta(To)
dX dY
...
Hx,Hy becomes the new estimated position
Reference
http://www.ece.cmu.edu/research/publications/2003/CMU-ECE-2003-038.pdf