Maxi.m

% Calibration data for third sensor
% 39 (measurement points) x 3 (measurements)
a = [[965 990 989],
     [914 933 933],
     [892 897 898],
     [796 853 853],
     [738 814 816],
     [693 787 789],
     [646 731 733],
     [597 719 724],
     [565 586 575],
     [530 544 537],
     [498 507 507],
     [460 479 478],
     [439 449 448],
     [414 423 423],
     [387 400 401],
     [365 376 380],
     [345 354 354],
     [327 330 335],
     [307 313 316],
     [296 302 296],
     [282 280 283],
     [267 269 272],
     [253 254 257],
     [242 245 245],
     [226 231 234],
     [220 221 223],
     [209 210 211],
     [202 202 205],
     [188 195 196],
     [184 187 187],
     [178 179 179],
     [168 171 171],
     [162 165 165],
     [156 159 159],
     [151 154 153],
     [147 146 146],
     [141 143 143],
     [136 138 138],
     [145 134 133]];
% Calibration data for second sensor
% 39 (measurement points) x 3 (measurements)
b = [[968 964 964],
     [906 904 898],
     [830 830 814],
     [753 754 740],
     [690 693 669],
     [622 619 606],
     [567 554 552],
     [511 514 505],
     [473 472 454],
     [428 429 415],
     [394 397 378],
     [363 363 347],
     [334 335 321],
     [309 309 297],
     [285 287 272],
     [270 271 253],
     [240 247 234],
     [234 234 220],
     [216 217 210],
     [202 203 200],
     [189 192 192],
     [181 180 179],
     [168 170 169],
     [159 160 159],
     [149 150 150],
     [144 143 141],
     [136 143 136],
     [128 129 128],
     [121 122 122],
     [115 117 116],
     [109 110 111],
     [104 104 105],
     [100 100 101],
     [95 96 95],
     [91 91 88],
     [87 88 88],
     [85 84 84],
     [81 81 80],
     [74 73 77]];
% Calibration data for first sensor
% 39 (measurement points) x 3 (measurements)
c = [[950 949 953]
     [868 871 873],
     [794 801 807],
     [739 737 737],
     [676 676 677],
     [619 620 619],
     [572 574 569],
     [525 526 527],
     [487 488 482],
     [450 451 449],
     [417 416 418],
     [389 388 386],
     [362 360 362],
     [340 339 335],
     [314 315 316],
     [295 293 297],
     [278 279 278],
     [263 262 264],
     [246 247 245],
     [234 233 233],
     [221 222 218],
     [209 208 208],
     [197 198 198],
     [188 189 188],
     [178 179 175],
     [170 171 170],
     [163 162 162],
     [155 157 155],
     [148 147 148],
     [141 142 141],
     [135 137 136],
     [129 128 131],
     [124 125 125],
     [119 120 121],
     [115 114 116],
     [111 112 113],
     [107 108 109],
  	 [104 105 105],
	   [103 103 103]];
% Measurements of distance in each part of the table
% 41 (width points) x 41 (height points) x 2 (x and y distance)
d = zeros(41, 41, 2);
d(1, :, :) = [[0, 0],
              [0, 0],
              [0, 10],
              [0, 12],
              [0, 14],
              [0, 17],
              [0, 19],
              [22, 0],
              [25, 0],
              [28, 0],
              [30, 0],
              [34, 0],
              [37, 0],
              [40, 0],
              [40, 0],
              [46, 2],
              [48, 3],
              [52, 5],
              [54, 5],
              [57, 6],
              [61, 7],
              [64, 8],
              [68, 11],
              [70, 10],
              [74, 12],
              [77, 13],
              [75, 32],
              [77, 33],
              [81, 32],
              [86, 30],
              [90, 29],
              [94, 27],
              [98, 25],
              [99, 22],
              [99, 20],
              [99, 18],
              [99, 15],
              [99, 13],
              [99, 11],
              [99, 1],
              [99, 1]];
d(2, :, :) = [[0, 1],
              [0, 8],
              [0, 9],
              [0, 11],
              [0, 13],
              [0, 15],
              [0, 18],
              [21, 0],
              [24, 0],
              [27, 0],
              [30, 0],
              [33, 0],
              [36, 1],
              [37, 0],
              [40, 2],
              [44, 3],
              [47, 3],
              [51, 5],
              [54, 6],
              [57, 6],
              [60, 8],
              [63, 9],
              [66, 10],
              [70, 11],
              [73, 13],
              [75, 13],
              [79, 15],
              [78, 30],
              [82, 29],
              [85, 29],
              [89, 27],
              [94, 26],
              [96, 24],
              [99, 22],
              [99, 19],
              [99, 17],
              [99, 15],
              [99, 12],
              [99, 11],
              [99, 9],
              [100, 9]];
d(3, :, :) = [[0, 9],
              [0, 9],
              [0, 10],
              [0, 11],
              [0, 13],
              [0, 16],
              [0, 18],
              [20, 0],
              [23, 0],
              [25, 1],
              [25, 1],
              [27, 1],
              [35, 3],
              [35, 11],
              [39, 4],
              [42, 6],
              [45, 6],
              [49, 8],
              [52, 9],
              [55, 10],
              [59, 11],
              [62, 12],
              [64, 12],
              [68, 14],
              [71, 14],
              [74, 15],
              [78, 18],
              [78, 28],
              [82, 27],
              [85, 28],
              [89, 26],
              [81, 20],
              [95, 23],
              [99, 21],
              [99, 19],
              [99, 17],
              [99, 15],
              [99, 13],
              [99, 11],
              [99, 10],
              [99, 11]];
d(4, :, :) = [[0, 12],
              [0, 11],
              [0, 11],
              [0, 12],
              [0, 14],
              [0, 16],
              [0, 18],
              [20, 1],
              [20, 1],
              [24, 3],
              [26, 3],
              [31, 5],
              [33, 5],
              [37, 6],
              [40, 8],
              [43, 8],
              [46, 9],
              [47, 9],
              [51, 10],
              [55, 11],
              [58, 12],
              [61, 13],
              [64, 14],
              [67, 15],
              [70, 15],
              [73, 17],
              [76, 19],
              [77, 26],
              [80, 28],
              [84, 27],
              [88, 25],
              [90, 25],
              [95, 24],
              [98, 22],
              [99, 20],
              [99, 17],
              [99, 16],
              [99, 14],
              [99, 13],
              [99, 12],
              [99, 13]];
d(5, :, :) = [[0, 14],
              [0, 13],
              [0, 13],
              [0, 14],
              [0, 15],
              [0, 17],
              [0, 19],
              [2, 21],
              [22, 5],
              [24, 6],
              [26, 6],
              [31, 7],
              [32, 7],
              [36, 9],
              [39, 10],
              [40, 9],
              [45, 10],
              [48, 11],
              [51, 12],
              [54, 13],
              [57, 14],
              [60, 14],
              [63, 15],
              [66, 17],
              [69, 17],
              [71, 19],
              [75, 20],
              [76, 28],
              [80, 25],
              [83, 26],
              [87, 24],
              [90, 25],
              [94, 24],
              [97, 22],
              [99, 20],
              [99, 19],
              [99, 17],
              [99, 16],
              [99, 15],
              [99, 14],
              [99, 15]];
d(6, :, :) = [[0, 16],
              [0, 16],
              [0, 16],
              [0, 16],
              [0, 17],
              [0, 19],
              [1, 18],
              [4, 19],
              [6, 5],
              [24, 8],
              [26, 9],
              [29, 9],
              [33, 10],
              [36, 11],
              [38, 12],
              [41, 13],
              [44, 13],
              [47, 14],
              [50, 14],
              [53, 15],
              [56, 15],
              [59, 16],
              [62, 17],
              [64, 18],
              [68, 19],
              [71, 21],
              [74, 21],
              [76, 25],
              [79, 27],
              [83, 24],
              [86, 26],
              [89, 25],
              [92, 24],
              [95, 23],
              [99, 22],
              [99, 20],
              [99, 19],
              [99, 17],
              [99, 17],
              [99, 17],
              [99, 17]];
d(7, :, :) = [[0, 19],
              [0, 18],
              [0, 18],
              [0, 18],
              [0, 19],
              [0, 20],
              [2, 19],
              [4, 7],
              [7, 7],
              [26, 11],
              [28, 12],
              [29, 12],
              [33, 13],
              [36, 13],
              [38, 13],
              [41, 15],
              [44, 15],
              [46, 16],
              [49, 16],
              [52, 17],
              [55, 18],
              [58, 18],
              [61, 19],
              [65, 20],
              [67, 22],
              [70, 24],
              [74, 22],
              [75, 27],
              [79, 27],
              [82, 27],
              [84, 26],
              [88, 26],
              [92, 25],
              [95, 24],
              [98, 23],
              [99, 21],
              [99, 20],
              [99, 20],
              [99, 19],
              [99, 19],
              [99, 19]];
d(8, :, :) = [[0, 22],
              [0, 21],
              [0, 20],
              [0, 20],
              [0, 21],
              [0, 20],
              [3, 20],
              [7, 23],
              [9, 23],
              [11, 11],
              [27, 14],
              [29, 15],
              [32, 16],
              [34, 17],
              [36, 16],
              [40, 18],
              [42, 18],
              [45, 18],
              [48, 19],
              [51, 20],
              [54, 20],
              [57, 20],
              [60, 22],
              [63, 22],
              [64, 24],
              [68, 24],
              [71, 25],
              [74, 27],
              [77, 26],
              [81, 26],
              [83, 26],
              [86, 27],
              [89, 26],
              [93, 25],
              [97, 24],
              [99, 23],
              [99, 22],
              [99, 21],
              [99, 21],
              [99, 21],
              [99, 22]];
d(9, :, :) = [[0, 25],
              [0, 24],
              [0, 23],
              [0, 23],
              [0, 23],
              [0, 15],
              [4, 22],
              [7, 25],
              [10, 24],
              [14, 27],
              [17, 27],
              [29, 18],
              [31, 19],
              [34, 19],
              [36, 20],
              [39, 20],
              [42, 21],
              [44, 21],
              [47, 22],
              [50, 23],
              [53, 22],
              [56, 24],
              [58, 25],
              [61, 26],
              [64, 26],
              [67, 26],
              [70, 28],
              [73, 28],
              [76, 28],
              [80, 29],
              [82, 29],
              [86, 28],
              [89, 27],
              [93, 27],
              [95, 26],
              [98, 25],
              [99, 24],
              [99, 24],
              [99, 24],
              [99, 24],
              [99, 25]];
d(10, :, :) = [[0, 28],
               [0, 26],
               [0, 26],
               [0, 26],
               [0, 25],
               [2, 23],
               [6, 27],
               [8, 27],
               [12, 28],
               [15, 27],
               [18, 28],
               [29, 21],
               [31, 21],
               [33, 22],
               [36, 22],
               [38, 23],
               [41, 24],
               [44, 24],
               [47, 24],
               [49, 25],
               [52, 26],
               [55, 26],
               [58, 26],
               [61, 27],
               [63, 28],
               [66, 29],
               [69, 28],
               [71, 29],
               [74, 29],
               [78, 29],
               [80, 30],
               [84, 29],
               [87, 30],
               [91, 28],
               [94, 28],
               [96, 27],
               [99, 27],
               [99, 27],
               [99, 26],
               [99, 27],
               [99, 28]];
d(11, :, :) = [[0, 31],
               [0, 32],
               [0, 29],
               [0, 28],
               [0, 28],
               [0, 24],
               [4, 29],
               [8, 29],
               [12, 30],
               [15, 30],
               [18, 30],
               [22, 30],
               [25, 30],
               [31, 25],
               [35, 26],
               [37, 26],
               [40, 27],
               [42, 28],
               [45, 27],
               [47, 28],
               [50, 28],
               [54, 29],
               [56, 31],
               [59, 31],
               [62, 31],
               [65, 30],
               [68, 31],
               [71, 32],
               [74, 32],
               [77, 32],
               [80, 32],
               [83, 30],
               [87, 31],
               [89, 30],
               [93, 30],
               [96, 30],
               [98, 30],
               [99, 29],
               [99, 29],
               [99, 30],
               [99, 31]];
d(12, :, :) = [[0, 37],
               [0, 34],
               [0, 33],
               [0, 31],
               [0, 31],
               [1, 26],
               [5, 31],
               [9, 31],
               [12, 32],
               [15, 32],
               [18, 32],
               [21, 32],
               [25, 32],
               [27, 33],
               [33, 31],
               [37, 29],
               [39, 30],
               [42, 30],
               [45, 30],
               [47, 32],
               [50, 32],
               [53, 32],
               [56, 32],
               [59, 32],
               [62, 33],
               [65, 33],
               [68, 33],
               [71, 33],
               [74, 33],
               [77, 33],
               [80, 33],
               [83, 34],
               [86, 32],
               [89, 33],
               [92, 32],
               [95, 32],
               [98, 32],
               [99, 32],
               [99, 32],
               [99, 33],
               [99, 33]];
d(13, :, :) = [[0, 37],
               [0, 36],
               [0, 35],
               [0, 34],
               [0, 29],
               [5, 34],
               [7, 34],
               [10, 35],
               [14, 34],
               [17, 34],
               [20, 34],
               [22, 34],
               [24, 35],
               [30, 33],
               [32, 34],
               [37, 32],
               [39, 33],
               [41, 33],
               [44, 33],
               [47, 34],
               [50, 34],
               [53, 35],
               [56, 35],
               [58, 35],
               [62, 35],
               [64, 35],
               [67, 36],
               [70, 35],
               [73, 35],
               [76, 34],
               [79, 36],
               [82, 35],
               [85, 35],
               [88, 35],
               [91, 34],
               [95, 35],
               [98, 34],
               [99, 35],
               [99, 35],
               [99, 36],
               [99, 37]];
d(14, :, :) = [[0, 41],
               [0, 39],
               [0, 38],
               [0, 38],
               [0, 36],
               [4, 37],
               [7, 36],
               [10, 37],
               [13, 36],
               [16, 36],
               [18, 36],
               [23, 36],
               [27, 36],
               [29, 36],
               [32, 36],
               [35, 36],
               [39, 35],
               [41, 35],
               [44, 36],
               [47, 35],
               [50, 37],
               [52, 36],
               [55, 36],
               [58, 36],
               [61, 37],
               [63, 37],
               [67, 36],
               [69, 37],
               [73, 37],
               [76, 38],
               [79, 37],
               [82, 37],
               [85, 36],
               [86, 37],
               [89, 36],
               [93, 37],
               [97, 37],
               [99, 37],
               [99, 38],
               [99, 39],
               [99, 40]];
d(15, :, :) = [[0, 44],
               [0, 42],
               [0, 41],
               [0, 40],
               [3, 38],
               [7, 39],
               [10, 39],
               [12, 38],
               [15, 39],
               [18, 38],
               [21, 38],
               [23, 39],
               [28, 38],
               [31, 38],
               [33, 38],
               [36, 38],
               [39, 38],
               [41, 39],
               [44, 38],
               [47, 38],
               [50, 39],
               [53, 38],
               [54, 39],
               [58, 39],
               [60, 39],
               [64, 39],
               [67, 38],
               [70, 40],
               [71, 40],
               [75, 40],
               [78, 40],
               [80, 40],
               [84, 39],
               [87, 41],
               [90, 39],
               [93, 39],
               [95, 40],
               [98, 40],
               [99, 41],
               [99, 41],
               [99, 42]];
d(16, :, :) = [[0, 47],
               [0, 46],
               [0, 44],
               [0, 43],
               [3, 43],
               [6, 42],
               [8, 42],
               [11, 42],
               [14, 42],
               [16, 42],
               [20, 41],
               [23, 41],
               [28, 40],
               [29, 41],
               [33, 40],
               [36, 40],
               [38, 41],
               [41, 40],
               [44, 41],
               [47, 40],
               [50, 41],
               [53, 42],
               [55, 41],
               [58, 41],
               [61, 41],
               [64, 41],
               [66, 43],
               [69, 43],
               [72, 42],
               [75, 43],
               [78, 43],
               [81, 42],
               [84, 42],
               [87, 42],
               [89, 42],
               [92, 43],
               [95, 43],
               [97, 43],
               [99, 44],
               [99, 45],
               [99, 46]];
d(17, :, :) = [[0, 50],
               [0, 48],
               [0, 47],
               [0, 44],
               [3, 44],
               [6, 44],
               [8, 45],
               [11, 44],
               [14, 44],
               [18, 44],
               [21, 44],
               [24, 44],
               [27, 43],
               [29, 43],
               [33, 43],
               [36, 43],
               [39, 43],
               [42, 43],
               [44, 43],
               [47, 43],
               [50, 44],
               [52, 43],
               [55, 44],
               [58, 43],
               [60, 43],
               [63, 43],
               [66, 43],
               [69, 43],
               [72, 44],
               [74, 45],
               [77, 44],
               [80, 44],
               [82, 45],
               [86, 44],
               [88, 45],
               [91, 45],
               [94, 45],
               [97, 46],
               [98, 46],
               [99, 47],
               [99, 48]];
d(18, :, :) = [[0, 54],
               [0, 52],
               [0, 51],
               [0, 49],
               [3, 49],
               [5, 47],
               [9, 48],
               [12, 47],
               [14, 47],
               [18, 47],
               [20, 47],
               [23, 46],
               [27, 46],
               [30, 46],
               [32, 46],
               [36, 46],
               [39, 46],
               [41, 46],
               [43, 46],
               [47, 46],
               [49, 46],
               [52, 46],
               [55, 46],
               [57, 46],
               [60, 46],
               [63, 47],
               [66, 46],
               [68, 46],
               [71, 45],
               [74, 45],
               [74, 45],
               [76, 47],
               [79, 46],
               [82, 46],
               [85, 47],
               [88, 47],
               [91, 48],
               [93, 48],
               [96, 49],
               [99, 50],
               [99, 52]];
d(19, :, :) = [[0, 54],
               [0, 53],
               [1, 51],
               [3, 49],
               [6, 48],
               [9, 48],
               [12, 48],
               [15, 47],
               [16, 47],
               [19, 46],
               [21, 46],
               [25, 47],
               [28, 46],
               [30, 47],
               [33, 46],
               [35, 46],
               [37, 47],
               [39, 47],
               [44, 46],
               [46, 46],
               [49, 46],
               [50, 48],
               [54, 46],
               [56, 46],
               [58, 48],
               [62, 46],
               [65, 46],
               [67, 46],
               [70, 46],
               [73, 47],
               [75, 48],
               [77, 47],
               [80, 48],
               [83, 48],
               [85, 48],
               [88, 50],
               [90, 49],
               [93, 50],
               [97, 51],
               [98, 52],
               [99, 53]];
d(20, :, :) = [[0, 60],
               [0, 57],
               [0, 55],
               [1, 53],
               [5, 54],
               [7, 53],
               [9, 52],
               [14, 51],
               [17, 51],
               [19, 51],
               [22, 51],
               [25, 50],
               [27, 50],
               [30, 50],
               [32, 50],
               [35, 50],
               [37, 50],
               [40, 50],
               [42, 50],
               [46, 50],
               [48, 50],
               [50, 50],
               [53, 50],
               [56, 51],
               [59, 50],
               [62, 50],
               [64, 50],
               [67, 51],
               [70, 50],
               [73, 50],
               [75, 51],
               [77, 51],
               [81, 51],
               [83, 52],
               [86, 52],
               [88, 52],
               [92, 53],
               [94, 54],
               [97, 55],
               [99, 57],
               [99, 58]];
d(21, :, :) = [[0, 64],
               [0, 61],
               [1, 58],
               [4, 58],
               [7, 58],
               [8, 57],
               [11, 56],
               [14, 55],
               [17, 55],
               [19, 54],
               [23, 54],
               [24, 54],
               [29, 53],
               [30, 53],
               [34, 53],
               [36, 53],
               [39, 53],
               [42, 53],
               [44, 53],
               [46, 53],
               [50, 53],
               [51, 52],
               [54, 53],
               [57, 53],
               [59, 53],
               [62, 53],
               [65, 53],
               [68, 54],
               [70, 54],
               [73, 53],
               [76, 54],
               [78, 54],
               [81, 54],
               [85, 55],
               [86, 55],
               [90, 56],
               [92, 56],
               [95, 57],
               [99, 58],
               [99, 59],
               [99, 61]];
d(22, :, :) = [[0, 67],
               [0, 63],
               [2, 61],
               [4, 60],
               [7, 59],
               [11, 59],
               [13, 58],
               [15, 57],
               [18, 57],
               [21, 56],
               [24, 56],
               [25, 56],
               [27, 56],
               [32, 56],
               [33, 56],
               [36, 55],
               [39, 55],
               [41, 56],
               [44, 56],
               [46, 56],
               [49, 56],
               [52, 56],
               [55, 56],
               [57, 56],
               [60, 56],
               [63, 56],
               [65, 56],
               [68, 56],
               [70, 56],
               [74, 56],
               [76, 57],
               [77, 57],
               [82, 57],
               [84, 58],
               [87, 58],
               [90, 58],
               [91, 59],
               [96, 61],
               [99, 61],
               [99, 63],
               [99, 64]];
d(23, :, :) = [[0, 70],
               [2, 68],
               [4, 66],
               [5, 65],
               [8, 64],
               [10, 62],
               [13, 62],
               [14, 61],
               [17, 61],
               [20, 60],
               [22, 60],
               [25, 59],
               [30, 59],
               [32, 59],
               [34, 58],
               [37, 59],
               [39, 59],
               [42, 59],
               [43, 58],
               [44, 56],
               [47, 56],
               [49, 56],
               [54, 55],
               [55, 56],
               [58, 55],
               [61, 56],
               [63, 56],
               [66, 56],
               [68, 56],
               [72, 57],
               [73, 57],
               [77, 57],
               [79, 58],
               [82, 58],
               [84, 58],
               [87, 60],
               [90, 61],
               [92, 62],
               [96, 63],
               [99, 64],
               [99, 66]];
d(24, :, :) = [[1, 72],
               [2, 67],
               [6, 68],
               [8, 67],
               [10, 66],
               [13, 64],
               [15, 63],
               [16, 63],
               [19, 62],
               [22, 61],
               [24, 62],
               [27, 61],
               [29, 60],
               [31, 60],
               [33, 60],
               [36, 60],
               [38, 60],
               [42, 61],
               [44, 60],
               [46, 60],
               [49, 60],
               [52, 60],
               [53, 60],
               [56, 60],
               [60, 60],
               [62, 60],
               [63, 60],
               [67, 60],
               [69, 61],
               [73, 61],
               [74, 61],
               [78, 61],
               [81, 62],
               [84, 62],
               [84, 63],
               [88, 63],
               [93, 65],
               [96, 66],
               [99, 67],
               [99, 68],
               [99, 70]];
d(25, :, :) = [[2, 75],
               [4, 73],
               [7, 72],
               [9, 70],
               [10, 68],
               [13, 67],
               [15, 67],
               [17, 66],
               [21, 65],
               [22, 65],
               [24, 64],
               [27, 64],
               [30, 64],
               [32, 64],
               [34, 63],
               [37, 64],
               [39, 63],
               [42, 63],
               [44, 63],
               [47, 63],
               [48, 63],
               [52, 63],
               [55, 63],
               [57, 63],
               [59, 63],
               [62, 63],
               [65, 63],
               [67, 63],
               [70, 64],
               [73, 64],
               [75, 64],
               [77, 64],
               [80, 65],
               [83, 65],
               [85, 65],
               [87, 67],
               [95, 68],
               [98, 69],
               [99, 70],
               [99, 72],
               [99, 73]];
d(26, :, :) = [[4, 80],
               [6, 77],
               [8, 76],
               [10, 74],
               [12, 73],
               [14, 72],
               [15, 70],
               [20, 69],
               [21, 69],
               [23, 68],
               [24, 68],
               [29, 67],
               [30, 67],
               [32, 67],
               [35, 66],
               [37, 66],
               [40, 67],
               [42, 66],
               [45, 66],
               [47, 66],
               [49, 66],
               [52, 66],
               [55, 66],
               [57, 66],
               [60, 66],
               [63, 66],
               [65, 66],
               [67, 67],
               [71, 67],
               [73, 66],
               [75, 68],
               [77, 67],
               [80, 68],
               [83, 68],
               [85, 69],
               [93, 71],
               [96, 72],
               [99, 73],
               [99, 74],
               [99, 74],
               [99, 76]];
d(27, :, :) = [[6, 83],
               [8, 81],
               [8, 79],
               [11, 77],
               [13, 76],
               [15, 75],
               [18, 73],
               [19, 73],
               [22, 72],
               [25, 71],
               [28, 70],
               [29, 70],
               [31, 69],
               [33, 70],
               [36, 69],
               [38, 69],
               [39, 69],
               [42, 69],
               [45, 68],
               [48, 69],
               [50, 69],
               [52, 69],
               [54, 69],
               [58, 69],
               [61, 68],
               [64, 69],
               [66, 68],
               [68, 69],
               [70, 70],
               [73, 70],
               [75, 69],
               [77, 70],
               [80, 70],
               [82, 72],
               [89, 73],
               [94, 75],
               [96, 75],
               [99, 75],
               [99, 76],
               [99, 78],
               [99, 79]];
d(28, :, :) = [[31, 82],
               [28, 80],
               [10, 82],
               [12, 81],
               [15, 79],
               [16, 78],
               [17, 76],
               [20, 75],
               [23, 74],
               [26, 74],
               [28, 73],
               [28, 73],
               [32, 72],
               [33, 72],
               [35, 72],
               [38, 71],
               [40, 71],
               [43, 71],
               [45, 72],
               [48, 71],
               [50, 71],
               [52, 71],
               [55, 71],
               [58, 71],
               [60, 71],
               [64, 70],
               [66, 71],
               [69, 71],
               [71, 72],
               [74, 70],
               [75, 73],
               [76, 74],
               [79, 74],
               [88, 76],
               [91, 77],
               [95, 77],
               [98, 78],
               [99, 79],
               [99, 80],
               [99, 81],
               [99, 83]];
d(29, :, :) = [[30, 85],
               [27, 84],
               [27, 82],
               [25, 82],
               [14, 82],
               [25, 79],
               [24, 78],
               [24, 78],
               [24, 78],
               [25, 77],
               [28, 76],
               [30, 76],
               [32, 76],
               [33, 75],
               [37, 75],
               [38, 75],
               [40, 74],
               [26, 59],
               [46, 74],
               [49, 74],
               [51, 74],
               [54, 74],
               [56, 75],
               [58, 74],
               [61, 74],
               [63, 74],
               [67, 73],
               [70, 73],
               [69, 75],
               [73, 74],
               [75, 76],
               [79, 74],
               [86, 79],
               [88, 79],
               [91, 80],
               [95, 81],
               [98, 81],
               [99, 83],
               [99, 84],
               [99, 85],
               [99, 86]];
d(30, :, :) = [[24, 84],
               [21, 83],
               [16, 82],
               [17, 81],
               [18, 79],
               [18, 78],
               [19, 77],
               [21, 76],
               [22, 76],
               [24, 75],
               [24, 75],
               [28, 73],
               [30, 74],
               [31, 74],
               [32, 74],
               [35, 73],
               [37, 73],
               [41, 73],
               [43, 73],
               [44, 73],
               [47, 73],
               [50, 72],
               [52, 72],
               [53, 71],
               [57, 72],
               [60, 72],
               [63, 71],
               [64, 72],
               [66, 73],
               [71, 71],
               [70, 74],
               [73, 74],
               [75, 75],
               [75, 76],
               [88, 79],
               [91, 80],
               [94, 81],
               [97, 83],
               [99, 84],
               [99, 85],
               [99, 87]];
d(31, :, :) = [[20, 85],
               [18, 84],
               [18, 82],
               [17, 81],
               [18, 80],
               [20, 78],
               [21, 77],
               [24, 76],
               [26, 75],
               [26, 75],
               [26, 75],
               [26, 75],
               [25, 76],
               [29, 75],
               [29, 75],
               [33, 74],
               [34, 74],
               [38, 73],
               [39, 74],
               [42, 73],
               [45, 73],
               [47, 73],
               [50, 73],
               [54, 72],
               [56, 72],
               [58, 72],
               [60, 72],
               [63, 72],
               [66, 72],
               [66, 73],
               [71, 71],
               [72, 73],
               [73, 73],
               [74, 74],
               [76, 76],
               [87, 80],
               [90, 81],
               [94, 82],
               [97, 84],
               [99, 85],
               [99, 87]];
d(32, :, :) = [[21, 89],
               [20, 88],
               [19, 87],
               [19, 86],
               [18, 84],
               [20, 83],
               [19, 82],
               [22, 81],
               [20, 81],
               [22, 80],
               [25, 80],
               [27, 79],
               [28, 79],
               [29, 79],
               [33, 78],
               [35, 78],
               [36, 78],
               [40, 78],
               [42, 77],
               [44, 77],
               [48, 76],
               [49, 76],
               [52, 75],
               [54, 76],
               [58, 76],
               [60, 76],
               [62, 75],
               [65, 74],
               [67, 75],
               [71, 74],
               [73, 74],
               [74, 75],
               [76, 76],
               [84, 83],
               [87, 83],
               [90, 84],
               [95, 86],
               [97, 87],
               [99, 88],
               [99, 89],
               [99, 91]];
d(33, :, :) = [[21, 94],
               [20, 92],
               [19, 91],
               [18, 88],
               [20, 87],
               [19, 87],
               [21, 85],
               [23, 85],
               [23, 84],
               [23, 84],
               [25, 83],
               [28, 82],
               [31, 82],
               [31, 82],
               [34, 81],
               [36, 81],
               [39, 80],
               [42, 80],
               [43, 80],
               [46, 80],
               [49, 80],
               [51, 79],
               [54, 79],
               [55, 79],
               [57, 79],
               [62, 79],
               [64, 78],
               [67, 78],
               [69, 77],
               [73, 77],
               [76, 76],
               [81, 85],
               [84, 86],
               [88, 87],
               [89, 87],
               [93, 88],
               [96, 90],
               [98, 90],
               [99, 92],
               [99, 93],
               [99, 94]];
d(34, :, :) = [[20, 98],
               [19, 96],
               [18, 94],
               [18, 93],
               [18, 92],
               [19, 91],
               [20, 90],
               [22, 88],
               [24, 88],
               [24, 88],
               [26, 87],
               [27, 86],
               [29, 85],
               [33, 85],
               [35, 85],
               [37, 85],
               [41, 83],
               [43, 83],
               [45, 83],
               [47, 82],
               [49, 82],
               [52, 83],
               [55, 82],
               [58, 81],
               [61, 81],
               [63, 81],
               [66, 80],
               [67, 80],
               [71, 80],
               [77, 88],
               [79, 88],
               [83, 89],
               [86, 90],
               [89, 90],
               [92, 91],
               [95, 92],
               [97, 93],
               [99, 94],
               [99, 95],
               [99, 96],
               [99, 99]];
d(35, :, :) = [[17, 99],
               [17, 99],
               [17, 98],
               [17, 96],
               [17, 94],
               [18, 94],
               [19, 93],
               [21, 92],
               [22, 91],
               [24, 91],
               [26, 90],
               [28, 90],
               [32, 88],
               [33, 88],
               [36, 87],
               [38, 87],
               [42, 86],
               [43, 86],
               [46, 86],
               [48, 85],
               [51, 85],
               [53, 85],
               [55, 84],
               [59, 84],
               [61, 83],
               [65, 83],
               [67, 82],
               [71, 81],
               [75, 90],
               [78, 91],
               [81, 92],
               [84, 93],
               [87, 93],
               [91, 94],
               [94, 95],
               [96, 95],
               [99, 97],
               [99, 97],
               [99, 99],
               [99, 99],
               [99, 99]];
d(36, :, :) = [[15, 99],
               [15, 99],
               [14, 99],
               [15, 99],
               [16, 99],
               [17, 98],
               [19, 96],
               [20, 95],
               [22, 95],
               [24, 94],
               [26, 92],
               [28, 92],
               [31, 91],
               [34, 91],
               [36, 90],
               [39, 89],
               [42, 89],
               [44, 89],
               [47, 90],
               [49, 89],
               [51, 87],
               [54, 88],
               [57, 87],
               [59, 87],
               [63, 86],
               [66, 85],
               [70, 93],
               [74, 94],
               [76, 94],
               [79, 94],
               [83, 96],
               [86, 97],
               [88, 96],
               [92, 97],
               [94, 98],
               [98, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99]];
d(37, :, :) = [[13, 99],
               [12, 99],
               [12, 99],
               [13, 99],
               [14, 99],
               [16, 99],
               [18, 99],
               [20, 99],
               [22, 99],
               [24, 97],
               [26, 96],
               [29, 96],
               [32, 95],
               [34, 94],
               [37, 93],
               [40, 93],
               [42, 92],
               [45, 92],
               [47, 91],
               [50, 90],
               [53, 91],
               [56, 89],
               [58, 89],
               [62, 88],
               [66, 96],
               [68, 95],
               [71, 96],
               [75, 97],
               [78, 97],
               [81, 98],
               [83, 98],
               [87, 99],
               [91, 99],
               [93, 99],
               [96, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99]];
d(38, :, :) = [[10, 99],
               [10, 99],
               [10, 99],
               [12, 99],
               [13, 99],
               [15, 99],
               [17, 99],
               [19, 99],
               [22, 99],
               [24, 99],
               [27, 99],
               [30, 98],
               [32, 96],
               [35, 97],
               [38, 95],
               [41, 95],
               [43, 95],
               [46, 95],
               [49, 94],
               [52, 93],
               [55, 93],
               [58, 92],
               [60, 91],
               [64, 98],
               [67, 99],
               [70, 99],
               [73, 99],
               [77, 99],
               [80, 99],
               [82, 99],
               [86, 99],
               [89, 99],
               [92, 99],
               [95, 99],
               [98, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99]];
d(39, :, :) = [[8, 99],
               [8, 99],
               [9, 99],
               [10, 99],
               [12, 99],
               [14, 99],
               [16, 99],
               [19, 99],
               [22, 99],
               [25, 99],
               [27, 99],
               [30, 99],
               [33, 99],
               [36, 99],
               [39, 99],
               [41, 98],
               [44, 97],
               [47, 96],
               [50, 96],
               [53, 96],
               [56, 95],
               [59, 99],
               [63, 99],
               [65, 99],
               [68, 99],
               [72, 99],
               [75, 99],
               [79, 99],
               [81, 99],
               [85, 99],
               [88, 99],
               [91, 99],
               [94, 99],
               [98, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99]];
d(40, :, :) = [[1, 99],
               [6, 99],
               [8, 99],
               [10, 99],
               [12, 99],
               [14, 99],
               [17, 99],
               [20, 99],
               [23, 99],
               [26, 99],
               [28, 99],
               [31, 99],
               [34, 99],
               [37, 99],
               [40, 99],
               [43, 99],
               [47, 99],
               [49, 99],
               [52, 98],
               [56, 99],
               [59, 99],
               [62, 99],
               [65, 99],
               [68, 99],
               [71, 99],
               [74, 99],
               [78, 99],
               [81, 99],
               [83, 99],
               [86, 99],
               [89, 99],
               [94, 99],
               [95, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [100, 99]];
d(41, :, :) = [[0, 100],
               [1, 100],
               [7, 99],
               [10, 99],
               [12, 99],
               [15, 99],
               [18, 99],
               [20, 99],
               [23, 99],
               [26, 99],
               [30, 99],
               [32, 99],
               [36, 99],
               [39, 99],
               [42, 99],
               [45, 99],
               [48, 99],
               [51, 99],
               [55, 99],
               [58, 99],
               [60, 99],
               [63, 99],
               [66, 99],
               [70, 99],
               [73, 99],
               [76, 99],
               [80, 99],
               [82, 99],
               [87, 99],
               [90, 99],
               [92, 99],
               [96, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [99, 99],
               [100, 100],
               [100, 100]];
% Measurement error matrix generation
z = zeros(41, 41);
for i = 1:41
  for j = 1:41
    z(i, j) = abs(sqrt(d(i, j, 1)^2 + d(i, j, 2)^2) - sqrt(((i-1) * 2.5)^2 + ((j-1) * 2.5)^2));
  endfor
endfor
% Drawing the measurement matrix as a heatmap
imagesc(z);
colormap hot;
colorbar;
% Calibration data for fourth sensor
% 39 (measurement points) x 3 (measurements)
f = [[974 974 973],
     [923 919 923],
     [866 866 871],
     [814 814 818],
     [766 765 766],
     [716 715 716],
     [672 671 670],
     [625 624 623],
     [587 587 585],
     [548 548 547],
     [518 516 516],
     [487 484 481],
     [457 454 452],
     [426 426 427],
     [404 401 402],
     [381 378 379],
     [360 357 355],
     [340 338 336],
     [322 317 320],
     [305 302 301],
     [289 288 286],
     [274 270 272],
     [263 257 261],
     [250 246 248],
     [239 237 232],
     [228 223 225],
     [218 215 215],
     [209 206 205],
     [200 198 197],
     [192 190 188],
     [184 181 180],
     [179 174 173],
     [170 167 166],
     [166 161 160],
     [160 155 154],
     [152 149 147],
     [147 144 142],
     [140 139 137],
     [140 139 137]];
% Plotting all calibration data
x = (1:39) .* (2.5 * sqrt(2));
a1 = a(:, 1)';
a2 = a(:, 2)';
a3 = a(:, 3)';
am = mean(a');
b1 = b(:, 1)';
b2 = b(:, 2)';
b3 = b(:, 3)';
bm = mean(b');
c1 = c(:, 1)';
c2 = c(:, 2)';
c3 = c(:, 3)';
cm = mean(c');
d1 = f(:, 1)';
d2 = f(:, 2)';
d3 = f(:, 3)';
dm = mean(f');
P1 = subplot(2, 2, 1);
P2 = subplot(2, 2, 2);
P3 = subplot(2, 2, 3);
P4 = subplot(2, 2, 4);
AX1 = plot(P1, x, a1, "r");
hold on;
grid on;
xlabel("Razdaljina [cm]");
ylabel("Merenje senzora");
title("Kalibracija prvog senzora");
AX2 = plot(P1, x, a2, "g");
AX3 = plot(P1, x, a3, "b");
AX4 = plot(P1, x, am, "m");
plot(P2, x, b1, "r");
hold on;
grid on;
xlabel("Razdaljina [cm]");
ylabel("Merenje senzora");
title("Kalibracija drugog senzora");
plot(P2, x, b2, "g");
plot(P2, x, b3, "b");
plot(P2, x, bm, "m");
plot(P3, x, c1, "r");
hold on;
grid on;
xlabel("Razdaljina [cm]");
ylabel("Merenje senzora");
title("Kalibracija treceg senzora");
plot(P3, x, c2, "g");
plot(P3, x, c3, "b");
plot(P3, x, cm, "m");
plot(P4, x, d1, "r");
hold on;
grid on;
xlabel("Razdaljina [cm]");
ylabel("Merenje senzora");
title("Kalibracija cetvrtog senzora");
plot(P4, x, d2, "g");
plot(P4, x, d3, "b");
plot(P4, x, dm, "m");

% Donji desni
d1 = zeros(6, 6, 2);
d1(1, :, :) = [[55.0, 55.0],
               [55.0, 56.0],
               [56.0, 56.0],
               [57.0, 56.0],
               [60.0, 53.0],
               [58.0, 56.0]];
d1(2, :, :) = [[54.0, 57.0],
               [58.0, 55.0],
               [59.0, 55.0],
               [57.0, 58.0],
               [60.0, 55.0],
               [58.0, 58.0]];
d1(3, :, :) = [[55.0, 58.0],
               [57.0, 57.0],
               [57.0, 58.0],
               [56.0, 59.0],
               [60.0, 56.0],
               [58.0, 59.0]];
d1(4, :, :) = [[55.0, 59.0],
               [57.0, 58.0],
               [58.0, 59.0],
               [57.0, 59.0],
               [60.0, 57.0],
               [61.0, 57.0]];
d1(5, :, :) = [[58.0, 58.0],
               [58.0, 58.0],
               [56.0, 60.0],
               [57.0, 60.0],
               [57.0, 61.0],
               [58.0, 61.0]];
d1(6, :, :) = [[57.0, 59.0],
               [55.0, 61.0],
               [58.0, 59.0],
               [57.0, 61.0],
               [57.0, 61.0],
               [60.0, 60.0]];
z1 = zeros(6, 6);
for i = 1:6
  for j = 1:6
    z1(i, j) = abs(sqrt(d1(i, j, 1)^2 + d1(i, j, 2)^2) - sqrt((50 - (i-1) * (2.5 / 6))^2 + (50 - (j-1) * (2.5 / 6))^2));
  endfor
endfor
imagesc(z1);
colormap hot;
colorbar;