[segway] vel

This test is done using params set 2, and decay 0.95.
First plot, ball moving trajetory.

Second plot, the x-y error on a 2D plot.

Note that in the above plot, we do not remove those cases when the ball is not in observation. When the ball is not visible,

vision.balls.objects[0].relpos.x = 0;
vision.balls.objects[0].relpos.y = last_y;

This will cause a big error between the prediction and the assumed observation. So this does not indicate a bad prediction at all. I removed those points and get the following plot.

When we change the prediction step T, the following plot shows how the mean and variance change.

[segway] observation

Based on the following table, maybe we want to use different gaussian models depending on how the far the ball is.

	obs-mean	obs-cov
0.79	0.7953 0.2288	1.0e-004 * 0.0084 0.0197 0.0197 0.1291
1.22	1.2227 0.2958	1.0e-004 * 0.1019 0.0393 0.0393 0.0191
1.52	1.5191 0.3285	1.0e-004 * 0.3333 0.1009 0.1009 0.0408
1.94	1.9478 0.3875	1.0e-003 * 0.1108 0.0323 0.0323 0.0106
2.21	2.2319 0.3525	1.0e-003 * 0.0914 0.0817 0.0817 0.1286
2.64	2.2310 0.3525	1.0e-003 * 0.1015 0.0869 0.0869 0.1430
2.81	2.7739 0.4569	1.0e-003 * 0.8690 0.1797 0.1797 0.0383
4.12	4.1177 -0.0114	1.0e-003 * 4.2677 0.0231 0.0231 0.0090

[segway] check vel

velocity-checking test
I put the ball on a ramp starting at (2.7, 0). The ball then rolls off the ramp with a speed roughly 0.75m/sec. The following graphs show the result.
- KF overshoots
- PF has a constant lag.





I should log the observations in each cycle! But why is the absolute velocity predicted so small? Should I replace .04 in the model with 1.0?? Or just multiply 25 to get the true speed? but why?

Test tonight:
- log observation
- tune constant in the model?
- look at the predicted location of the ball some fixed time step, T, into the future.
- Compare the resulting location of the ball observed at T.
- Do this for a range of observations
- plot the x-y error on a 2D plot and calculate mean and variance in x and y.

[segway] Tracking Report I

System model and observation model.

 
/*     x(t)   =     A      *      x(t-1)   +  v(t-1)
 * [  x1(t) ] = [ 1 0 .04 0 ] [  x1(t-1) ] + [  wx1 ] 
 * [  x2(t) ]   [ 0 1  0 .04] [  x2(t-1) ]   [  wx2 ]
 * [ dx1(t) ]   [ 0 0 .99 0 ] [ dx1(t-1) ]   [ wdx1 ]
 * [ dx2(t) ]   [ 0 0  0 .99] [ dx2(t-1) ]   [ wdx2 ]
 *
 *     y(t)   =     C      *    x(t-1)   +      D       *    u2(t)   +  n(t)
 * [  y1(t) ] = [ 1 0 0 0 ] [  x1(t-1) ] + [ -1  0 0 0 ] [  rx1(t) ] + [ vx1 ]
 * [  y2(t) ]   [ 0 1 0 0 ] [  x2(t-1) ]   [  0 -1 0 0 ] [  rx2(t) ]   [ vx2 ]
 *                          [ dx1(t-1) ]                 [ drx1(t) ]
 *                          [ dx2(t-1) ]                 [ drx2(t) ]
 */

cycle duration: 0.04sec
ball vel decay: 0.99
Particle filter uses SIR algorithm.

I set the segway at unbalanced mode. This is the static ball test setup.

The origin is at the center of the wheels.

Robot's view when I put the ball at (.9, 0).

I collected the result of kalman filter and particle filter when the ball is at (.9, 0) and (2.7, 0) respectively. From the result, it seems KF is better in terms of standard error when the ball is closer. When the ball is far away, PF is better. But we may need more tests to conclude.

	# data	KF mean	KF std	PF mean	PF std
(0.9, 0)	2243	(0.7488, 0.3392)	(0.0005, 0.0009)	(0.7489, 0.3393)	(0.0018,0.0018)
(2.7,0)	7210	(3.9065,0.3168)	(0.4610,0.2817)	(3.9722,0.2829)	(0.0679,0.3811)

Moving-ball test setup. I kick the ball back and forth between (3.6, 0) and (3.6, -2.7). Note these two points are observed by the segway as (4.20, 0.15) and (3,45, -0.94) in the current setup. I run the test for about 3 minutes (4212 cycles) and record the tracking result of KF and PF.

Below are 4 selected predicted trajectory of KF and PF during 100 cycles. In each plot, x/y axis is the predicted x/y relative position of the ball, the blue dot denotes the KF prediction and the red cross represents the PF prediction. And the circle is the starting point, while the star is the ending point. Though we do not know exactly the true value of each intermediate position of the ball, I kick the ball straightly and try to keep it rolling in a straight line (directly from (3.6, 0) to (3.6, -2.7). In each plot, it is obvious that PF prediction is much smoother than that of KF.






The other new feature of this PF implementation is the velocity estimation. Here are two sets of plot.
The below one is similar to the above 4 but it is during 50 cycles.

This is the corresponding velocity estimation from PF. x and y axis are predicted v_x, v_y of the ball.

Here is another set.


It can be seen that the predicted velocity is quite usable in these two cases.

TODO:
- test: moving robot observes static ball
- test: moving robot observes moving ball
- handle with false positive
- better movement model
- Next: tracking markers and self-localization by PF

[segway] pf draft

• world::world() ball pf tracker init
• world::updateVision(...) ball pf tracker update
• world::getBallPosition() ball pf tracker estimate
• world::getBallVelocity() ball pf tracker estimate

[debian] recursively grep

search directories recursively and print line number

# grep -rn 'IEEE1394' ./

[segway] forward declaration

from http://www-subatech.in2p3.fr/~photons/
subatech/soft/carnac/CPP-INC-1.shtml
1. B only uses references or pointers to A. Use forward declaration then : you don't need to include . This will in turn speed a little bit the compilation.

  class A ;
  
  class B {
    private:
      A* fPtrA ;
    public:
      void mymethod(const& A) const ;
  } ;

2. B derives from A or B explicitely (or implicitely) uses objects of class A. You then need to include

  #include 
  
  class B : public A {
  
  } ;
  
  class C {
    private:
      A fA ;
    public:
      void mymethod(A par) ;   
  }

[debian] purge remove

# apt-get remove libfltk1.1

Running apt-get as above will cause the packages to be removed but their configuration files, if any, will remain intact on the system. For a complete removal of the package, run:

# apt-get --purge remove libfltk1.1

[debian] change default shell

as root:

# vipw

(this will open up the password file where shells are defined per user) or

# chsh -s /usr/local/bin/bash' 

(this will change the shell for the current user to bash.)

[debian] apt-get install error

After unpacking 197kB of additional disk space will be used.
(Reading database ... 173333 files and directories currently installed.)
Unpacking sysv-rc (from .../sysv-rc_2.86.ds1-1_all.deb) ...
dpkg: error processing /var/cache/apt/archives/sysv-rc_2.86.ds1-1_all.deb (--unpack):
trying to overwrite `/etc/init.d/rc', which is also in package sysvinit
dpkg-deb: subprocess paste killed by signal (Broken pipe)

solution:

# dpkg -i --force-overwrite  
/var/cache/apt/archives/sysv-rc_2.86.ds1-1_all.deb

[debian] couldn't connect to display

happens after su

solution:

test@localhost:~$ su
root@localhost:~# xauth merge ~test/.Xauthority
root@localhost:~# export DISPLAY=:0.0
root@localhost:~# xterm # it should be fine now

reference

[segway] parameter collection for segway sim

// Camera field of view in horizontal and vertical
// in degrees

CAMERA_FOV = 94.8 70.6 // degrees

//
// This is the position of the camera with respect to the robot's
// coordinate frame (on the ground, not between the wheels)
// This is a translation of that point...

CAMERA_TRANSLATION = 0.235, 0, 1.405 // m [x, y, z] measured

//------- pan tilt settings -------------
SWAY_PAN_MIN_ANGLE = -90.0 // degrees
SWAY_PAN_MAX_ANGLE = 90.0 // degrees

SWAY_TILT_MIN_ANGLE = -88.0 // # degrees
SWAY_TILT_MAX_ANGLE = 90.0 // # degrees