laser_data_fisher.c
Go to the documentation of this file.
00001 #include <math.h>
00002 #include <egsl/egsl_macros.h>
00003 
00004 #include "csm_all.h"
00005 
00014 val ld_fisher0(LDP ld) {
00015         val fim   = zeros(3,3);
00016         int i;
00017         for(i=0;i<ld->nrays;i++) {              
00018                 double alpha = ld->true_alpha[i];
00019                 if(is_nan(alpha)) continue;
00020                 
00021                 double theta = ld->theta[i];
00022                 double beta  = alpha-theta;
00023                 
00024                 double r = ld->readings[i];
00025                 double c = cos(alpha);
00026                 double s = sin(alpha);
00027                 
00028                 double z = 1 / cos(beta);
00029                 double t = tan(beta);
00030                 
00031                 double fim_k[9] ={
00032                         c*c*z*z,    c*s*z*z,   c*z *t*r,
00033                         c*s*z*z,    s*s*z*z,   s*z *t*r, 
00034                         c*z*t*r,    s*z*t*r,   t*r *t*r 
00035                 };
00036                 
00037                 egsl_push();
00038                         val k = egsl_vFda(3,3,fim_k);
00039                         add_to(fim, k);
00040                 egsl_pop();
00041         }
00042         return fim;
00043 }


csm_ros
Author(s): Gaƫl Ecorchard , Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Sat Jun 8 2019 19:52:42