Page 247 - Introduction to Autonomous Mobile Robots
P. 247

Chapter 5
                           232
                             The new, fused estimate of robot position provided by the Kalman filter is again subject
                           to a Gaussian probability density curve. Its mean and variance are simply functions of the
                           inputs’ means and variances. Thus the Kalman filter provides both a compact, simplified
                           representation of uncertainty and an extremely efficient technique for combining heteroge-
                           neous estimates to yield a new estimate for our robot’s position.

                           Dynamic estimation. Next, consider a robot that moves between successive sensor mea-
                           surements. Suppose that the motion of the robot between times k  and k +  1   is described
                           by the velocity u and the noise w which represents the uncertainty of the actual velocity:


                                dx
                                ------ =  u +  w                                             (5.41)
                                dt
                             If we now start at time  , knowing the variance σ 2 k   of the robot position at this time and
                                               k
                           knowing the variance σ 2 w  of the motion, we obtain for the time  k′   just when the measure-
                           ment is taken,


                                          (
                                x ˆ k′  =  x ˆ +  ut k +  1  –  t )                          (5.42)
                                                k
                                      k
                                      2
                                          2
                                            [
                                σ 2 k′  =  σ +  σ t k +  1  – t ]                            (5.43)
                                                  k
                                      k
                                          w
                           where
                             t  =  t  ;
                              k '  k +  1
                             t    and   are the time in seconds at k +  1   and   respectively.
                                                                    k
                                     t
                              k +  1  k
                             Thus  x ˆ k′   is the optimal prediction of the robot’s position just as the measurement is
                           taken at time k +  1  . It describes the growth of position error until a new measurement is
                           taken (figure 5.27).
                             We can now rewrite equations (5.38) and (5.39) using equations (5.42) and (5.43).
                                x ˆ  =  x ˆ +  K  z (  – x ˆ )
                                 k +  1  k′  k +  1  k +  1  k′
                                                (
                                                                          (
                                    x ˆ  k +  1  =  [ x ˆ +  ut k +  1  –  t )] +  K  k +  1  z [  k +  1  –  x ˆ –  ut k +  1  –  t )]   (5.44)
                                                                                 k
                                                                       k
                                                       k
                                            k
                                                        2
                                                         [
                                         σ 2       σ +  σ t   –  t ]
                                                    2
                                          k′
                                                          k +
                                                                k
                                                            1
                                                    k
                                                        w
                                K    =  ------------------- =  --------------------------------------------------------   (5.45)
                                 k + 1  2    2    2   2           2
                                                       [
                                       σ + σ z   σ +  σ t k +  1  –  t ] + σ z
                                        k′
                                                              k
                                                      w
                                                  k
   242   243   244   245   246   247   248   249   250   251   252