Added a new program to interpolate position every 0.1s
authorBenoit Papillault <benoit.papillault@free.fr>
Sun, 17 Jul 2011 07:03:09 +0000 (09:03 +0200)
committerBenoit Papillault <benoit.papillault@free.fr>
Sun, 17 Jul 2011 07:03:09 +0000 (09:03 +0200)
Added a script to convert old mygps log to a readable format
Added a file with gnuplot formula / URL for 2 rays propagation
Updated output format of mygps

gps_interpolate.c [new file with mode: 0644]
mygps.c
propagation.txt [new file with mode: 0644]
script-mygps.sh [new file with mode: 0755]

diff --git a/gps_interpolate.c b/gps_interpolate.c
new file mode 100644 (file)
index 0000000..0249c4c
--- /dev/null
@@ -0,0 +1,182 @@
+/*
+  Author : Benoit Papillault <benoit.papillault@free.fr>
+  Creation : 2011-07-13
+
+  Goal : interpolate position every 100ms from a GPS log every second
+    or so
+
+  awk '{print $1" "$2" "$3}' < mygps-tgv-aller.txt  | cut -c19- > y.txt
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <gps.h>
+
+struct position {
+  /* read from log file */
+  double time;
+  double longitude;
+  double latitude;
+
+  /* computed */
+  double distance;     /* distance to first position */
+  double speed;                /* average speed with prev position, valid for i>0 */
+  double acceleration; /* average acceleration with prev speed, valid for i>1 */
+};
+
+int main() {
+    const char file[] = "y.txt";
+    FILE * fp;
+    double time, longitude, latitude;
+    int i,j,k;
+    double ref_latitude = 49.169817, ref_longitude = 2.304467;
+
+    struct position * position_list = NULL;
+    int position_num = 0;
+
+    fp = fopen(file, "r");
+    if (fp == NULL) {
+      perror(file);
+      exit (-1);
+    }
+
+    /* each line should be : time_t longitude latitude */
+
+    while (fscanf(fp, "%lf %lf %lf", &time, &longitude, &latitude) != EOF) {
+      /*printf("time : %f longitude : %f latitude : %f\n", time, longitude, latitude);*/
+
+      /* allocate a new element in the array, if needed */
+      if ((position_num==0) || (time > position_list[position_num-1].time)) {
+       position_list = (struct position *)realloc(position_list, (++position_num)*sizeof(struct position));
+       if (!position_list) {
+         perror("realloc");
+         exit (-1);
+       }
+      }
+
+      position_list[position_num-1].time = time;
+      position_list[position_num-1].longitude = longitude;
+      position_list[position_num-1].latitude = latitude;
+    }
+
+    fclose (fp);
+
+    /* process the array */
+
+    for (i=0; i<position_num; i++) {
+
+      /* initialize with default values */
+      position_list[i].distance                = 0.0;
+      position_list[i].speed           = 0.0;
+      position_list[i].acceleration    = 0.0;
+
+      /* compute distance with first position */
+      position_list[i].distance = earth_distance(position_list[i].latitude, position_list[i].longitude,
+                                                ref_latitude, ref_longitude);
+
+      if (i>0) {
+       position_list[i].speed = (position_list[i].distance - position_list[i-1].distance) /
+         (position_list[i].time - position_list[i-1].time);
+      }
+
+      if (i>1) {
+       position_list[i].acceleration = (position_list[i].speed - position_list[i-1].speed) /
+         (position_list[i].time - position_list[i-1].time);
+      }
+
+      /* time(s) distance(m) speed(m/s) acceleration(m/s²) */
+      printf(
+            "time %f distance %13f speed %10f acceleration %10f\n",
+            position_list[i].time, position_list[i].distance,
+            position_list[i].speed, position_list[i].acceleration);
+    }
+
+    /* compute an interpolation for the first 2 seconds, every 100 ms
+       : 20 values.
+       j is like position_list[j].time <= time < position_list[j+1].time
+    */
+
+    double start = 1310643040.0;
+
+    for (i=0,j=0; i<100; i++) {
+      double weight;
+      double distance1, distance2;
+
+      time = start + i * 0.100;
+      for (;(j+1<position_num) && (time >= position_list[j+1].time);j++) {}
+
+      printf("interpolating for time %f i: %2d j: %2d %f <= %f < %f\n",
+            time, i, j, position_list[j].time, time, position_list[j+1].time);
+
+      weight = (time - position_list[j].time) / (position_list[j+1].time - position_list[j].time);
+
+      printf("  => weight = %f\n", weight);
+
+      longitude = position_list[j].longitude + (position_list[j+1].longitude - position_list[j].longitude) * weight;
+
+      latitude = position_list[j].latitude + (position_list[j+1].latitude - position_list[j].latitude) * weight;
+
+      distance1 = earth_distance(latitude, longitude, 
+                                position_list[j].latitude, position_list[j].longitude);
+
+      distance2 = earth_distance(latitude, longitude,
+                                position_list[j+1].latitude, position_list[j+1].longitude);
+
+      printf("  => position %f,%f (%f m) <= %f,%f < %f,%f (%f m) [%f m]\n",
+            position_list[j].longitude, position_list[j].latitude, distance1, 
+            longitude, latitude,
+            position_list[j+1].longitude, position_list[j+1].latitude, distance2,
+            distance1 + distance2);
+
+      /*
+       Fiona Gellin / album Passeport
+       f(t) = a * t + b
+       f(t0) = f0 = a * t0 + b
+       f(t1) = f1 = a * t1 + b
+
+       f(t1) - f(t0) = f1 - f0 = a * (t1 - t0)
+       => a = (f1 - f0) / (t1 - t0)
+       => b = (f0 - a * t0) = (f0 - (f1 - f0) / (t1 -t0) * t0)
+            = (f0 * (t1 - t0) - (f1 - f0) * t0) / (t1 - t0)
+            = (f0*t1 - f0*t0 - f1*t0 + f0*to) / (t1 - t0)
+            = (f0*t1 - f1-t0) / (t1 - t0)
+
+       We have computed d, v and a at discrete time t0, t1, t2
+
+       a(t) = a(ti+1) for t€[ti,ti+1]
+       v(t) = v(ti) + a(t).(t-ti) for t€[ti,ti+1]
+       d(t) = d(ti) + v(t).(t-ti) for t€[ti,ti+1]
+
+       using above formulas, we have :
+
+       a(ti) = a(ti)
+       v(ti) = v(ti)
+       d(ti) = d(ti)
+
+       still using above formulas, we have :
+
+       a(ti+1) = a(ti) (acceleration is not continuous)
+       v(ti+1) = v(ti) + a(ti+1).(ti+1-ti)
+       d(ti+1) = d(ti) + (v(ti) + a(ti+1).(ti+1-ti)).(ti+1-ti)
+
+       in practice, here is how we computed v(ti+1) and a(ti+1)
+
+       v(t0)   = 0
+       v(ti+1) = (d(ti+1)-d(ti))/(ti+1-ti)
+         => d(ti+1) = d(ti) + v(ti+1).(ti+1-ti)
+         => d(t)    = d(ti) + v(t).(t-ti)
+
+       a(t0)   = 0
+       a(t1)   = 0
+       a(ti+1) = (v(ti+1)-v(ti))/(ti+1-ti)
+         => v(ti+1) = v(ti) + a(ti+1).(ti+1-ti)
+         => v(t)    = v(ti) + a(ti+1).(t-ti)
+
+       so we can simplify the above formulas :
+       v(ti+1
+      */
+
+    }
+
+    return 0;
+}
diff --git a/mygps.c b/mygps.c
index 146142d..1ce0479 100644 (file)
--- a/mygps.c
+++ b/mygps.c
@@ -314,7 +314,7 @@ int main(int argc, const char *argv[]) {
                 <horizontal speed error in m/s>
                 <vertical speed error in m/s>
              */
-             printf("%u.%06lu-%f %f %f %f %f %f %f %f %f %f %f %f\n",
+             printf("%u.%06lu-%f %10f %10f %10f %10f %10f %10f %10f %10f %10f %10f %10f\n",
                     /* time recv */
                     tv_now.tv_sec, tv_now.tv_usec, gps->fix.time,
                     /* longitude in °, latitude in °, altitude in m */
diff --git a/propagation.txt b/propagation.txt
new file mode 100644 (file)
index 0000000..6a4546c
--- /dev/null
@@ -0,0 +1,3 @@
+gnuplot> plot [x=0:2000] f(x) = -20*log(4*pi*17*x)+10*log(2-2*cos(4*pi*17/x*5*5)), g(x) = -20*log(4*pi*17*x), f(x), g(x)
+
+http://www.scribd.com/doc/31684964/Modeles-de-Propagation-en-GSM
diff --git a/script-mygps.sh b/script-mygps.sh
new file mode 100755 (executable)
index 0000000..3d7f34b
--- /dev/null
@@ -0,0 +1,12 @@
+#!/bin/sh
+
+AWK='
+/^Fix time:/ {
+       T=$3
+}
+/^Latitude:/ {
+       print T" "$5" "$2
+}
+'
+
+awk "${AWK}" < x