Reference is needed for Optical flow sensor culculation through kalman filter
Posted: Mon Mar 19, 2018 10:11 am
Hello every body ! I need Scientific Reference for Optical flow sensor calculation through kalman filter that is used in the following code
where does this equation come from? what is the reference for this !!
and thank you very much
/////////////////////////////////////////////////////////////////////
static void s t a t e E s t i m a t o r U p d a t e W i t h F l o w ( f lowMe asurem ent_t * flow ,
sensorData_t * sensors )
// ~~~ X velocity prediction and update ~~~
// predics the number of accumulated pixels in the x - direction
float omegaFactor = 1.25 f ;
float hx [ STATE_DIM ] = {0};
a rm _ m a t r i x _ i n s t a n c e _ f 3 2 Hx = {1 , STATE_DIM , hx };
predictedNX = ( flow - > dt * Npix / thetapix ) *
(( dx_g * R [2][2] / z_g ) - omegaFactor * omegay_b );
measuredNX = flow - > dpixelx ;
// derive measurement equation with respect to dx ( and z )
hx [ STATE_Z ] = ( Npix * flow - > dt / thetapix ) *
(( R [2][2] * dx_g ) / ( - z_g * z_g ));
hx [ STATE_PX ] = ( Npix * flow - > dt / thetapix ) *
( R [2][2] / z_g );
/ First update
s t a t e E s t i m a t o r S c a l a r U p d a t e (& Hx , measuredNX - predictedNX ,
flow - > stdDevX );
// ~~~ Y velocity prediction and update ~~~
float hy [ STATE_DIM ] = {0};
a rm _ ma tr i x _ i n s t a n c e _ f 3 2 Hy = {1 , STATE_DIM ,
hy };
predictedNY = ( flow - > dt * Npix / thetapix )
* (( dy_g * R [2][2] / z_g ) + omegaFactor *
omegax_b );
measuredNY = flow - > dpixely ;
where does this equation come from? what is the reference for this !!
and thank you very much
/////////////////////////////////////////////////////////////////////
static void s t a t e E s t i m a t o r U p d a t e W i t h F l o w ( f lowMe asurem ent_t * flow ,
sensorData_t * sensors )
// ~~~ X velocity prediction and update ~~~
// predics the number of accumulated pixels in the x - direction
float omegaFactor = 1.25 f ;
float hx [ STATE_DIM ] = {0};
a rm _ m a t r i x _ i n s t a n c e _ f 3 2 Hx = {1 , STATE_DIM , hx };
predictedNX = ( flow - > dt * Npix / thetapix ) *
(( dx_g * R [2][2] / z_g ) - omegaFactor * omegay_b );
measuredNX = flow - > dpixelx ;
// derive measurement equation with respect to dx ( and z )
hx [ STATE_Z ] = ( Npix * flow - > dt / thetapix ) *
(( R [2][2] * dx_g ) / ( - z_g * z_g ));
hx [ STATE_PX ] = ( Npix * flow - > dt / thetapix ) *
( R [2][2] / z_g );
/ First update
s t a t e E s t i m a t o r S c a l a r U p d a t e (& Hx , measuredNX - predictedNX ,
flow - > stdDevX );
// ~~~ Y velocity prediction and update ~~~
float hy [ STATE_DIM ] = {0};
a rm _ ma tr i x _ i n s t a n c e _ f 3 2 Hy = {1 , STATE_DIM ,
hy };
predictedNY = ( flow - > dt * Npix / thetapix )
* (( dy_g * R [2][2] / z_g ) + omegaFactor *
omegax_b );
measuredNY = flow - > dpixely ;