petersaints

Location Position Angle Estimation

Oct 21st, 2020
711
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. const combinations = require('combinations');
  2. const { euclidean } = require('ml-distance-euclidean');
  3. const { dot } = require('mathjs');
  4.  
  5. const degToRad = value => value * Math.PI / 180;
  6. const radToDeg = value => value * 180 / Math.PI;
  7.  
  8. const locations = [
  9.     {
  10.         deviceUuid: "9ab8e750-bc1e-11e8-a769-3f2e91eebf08",
  11.         username: "test_user_0@yanux.org",
  12.         position: {
  13.             x: 1, y: 2, orientation: 30,
  14.             place: "/app/p37.json",
  15.         }
  16.     },
  17.     {
  18.         deviceUuid: "9ab8e750-bc1e-11e8-a769-3f2e91eebf09",
  19.         username: "test_user_0@yanux.org",
  20.         position: {
  21.             x: 2, y: 3, orientation: -30,
  22.             place: "/app/p37.json",
  23.         }
  24.     },
  25.     {
  26.         deviceUuid: "9ab8e750-bc1e-11e8-a769-3f2e91eebf10",
  27.         username: "test_user_0@yanux.org",
  28.         position: {
  29.             x: 2, y: 3, orientation: 0,
  30.             place: "/app/p37.json",
  31.         }
  32.     }
  33. ]
  34.  
  35. locations.forEach(l => {
  36.     if (l.position && (l.position.orientation !== undefined || l.position.orientation !== null)) {
  37.         const orientationRad = degToRad(l.position.orientation);
  38.         l.position.headingVector = [Math.cos(orientationRad), Math.sin(orientationRad)];
  39.     }
  40. });
  41.  
  42. const locationPairs = combinations(locations, 2, 2);
  43. locationPairs.forEach(([l1, l2]) => {
  44.     if (l1.position && l1.position.headingVector && l2.position.headingVector &&
  45.         l1.position.x && l1.position.y && l2.position.x && l2.position.y) {
  46.         const dotProduct = dot(l1.position.headingVector, l2.position.headingVector);
  47.         const dotProductAngle = radToDeg(Math.acos(dotProduct))
  48.         const distance = euclidean([l1.position.x, l1.position.y], [l2.position.x, l2.position.y])
  49.         console.log(
  50.             '-------- Computed Values for L1 (' + l1.deviceUuid + ') and L2 (' + l2.deviceUuid + ') --------',
  51.             '\nl1.position.headingVector:', l1.position.headingVector,
  52.             '\nl2.position.headingVector:', l2.position.headingVector,
  53.             '\n',
  54.             '\ndistance:', distance, 'dotProduct:', dotProduct, 'dotProductAngle:', dotProductAngle,
  55.             '\n----------------------------------------------------------------'
  56.         );
  57.     }
  58. });
RAW Paste Data