Commit 2e71b4d2 by Torbjørn Ludvigsen 👷

### Static Z Kokokaka

parent 13ac0f3b
 ... ... @@ -16,19 +16,16 @@ X = 0 Y = 1 Z = 2 def symmetric_anchors(l): def symmetric_anchors(l, az=-120., bz=-120., cz=-120.): anchors = np.array(np.zeros((4, 3))) #anchors[A, X] = 0 # Fixated anchors[A, Y] = -l #anchors[A, Z] = 0 # Fixated anchors[A, Z] = az anchors[B, X] = l*np.cos(np.pi/6) anchors[B, Y] = l*np.sin(np.pi/6) #anchors[B, Z] = 0 # Fixated anchors[B, Z] = bz anchors[C, X] = -l*np.cos(np.pi/6) anchors[C, Y] = l*np.sin(np.pi/6) #anchors[C, Z] = 0 # Fixated #anchors[D, X] = 0 # Fixated #anchors[D, Y] = 0 # Fixated anchors[C, Z] = cz anchors[D, Z] = l return anchors ... ... @@ -36,7 +33,7 @@ def centered_rand(l): """Sample from U(-l, l)""" return l*(2.*np.random.rand()-1.) def irregular_anchors(l, fuzz_percentage = .2): def irregular_anchors(l, fuzz_percentage = .2, az=-120., bz=-120.,cz=-120.): """Realistic exact positions of anchors. Each dimension of each anchor is treated separately to ... ... @@ -52,7 +49,6 @@ def irregular_anchors(l, fuzz_percentage = .2): (except Z-difference of B- and C-anchors) """ fuzz = np.array(np.zeros((4, 3))) #fuzz[A, X] = 0 # Fixated fuzz[A, Y] = centered_rand(l*fuzz_percentage) #fuzz[A, Z] = 0 # Fixated fuzz[B, X] = centered_rand(l*fuzz_percentage*np.cos(np.pi/6)) ... ... @@ -64,7 +60,7 @@ def irregular_anchors(l, fuzz_percentage = .2): #fuzz[D, X] = 0 # Fixated #fuzz[D, Y] = 0 # Fixated fuzz[D, Z] = l*fuzz_percentage*np.random.rand() # usually higher than A is long return symmetric_anchors(l)+fuzz return symmetric_anchors(l, az, bz, cz)+fuzz def positions(n, l, fuzz=10): """Return (n^3)x3 matrix of positions in fuzzed grid of side length 2*l ... ... @@ -195,25 +191,25 @@ def solve(samp, cb, _cost = cost_sq, az = 0., bz = 0., cz = 0.): number_of_params_anch = 6 # Define bounds lb = [ -l_long, # A_ay > -5000.0 -l_long*cos30, # A_bx > -5000*cos(30) lb = [ -l_long, # A_ay > -4000.0 0, # A_bx > 0 0, # A_by > 0 0, # A_cx > 0 -l_long*cos30, # A_cx > -4000*cos(30) 0, # A_cy > 0 0, # A_dz > 0 -50.0, # x0 > -50.0 -50.0, # y0 > -50.0 -10, # z0 > -10 ] + [-l_short, -l_short, -10]*(u-1) -0.1, # x0 > -0.1 -0.1, # y0 > -0.1 -0.1, # z0 > -0.1 ] + [-l_short, -l_short, -10.]*(u-1) ub = [ 0, # A_ay < 0 0, # A_bx < 0 l_long*sin30, # A_by < 5000.0*sin(30) l_long*cos30, # A_cx < 5000.0*cos(30) l_long*sin30, # A_cy < 5000.0*sin(30) l_long, # A_dz < 10000.0 50.0, # x0 < 50.0 50.0, # y0 < 50.0 l_short, # z0 < l_short l_long*cos30, # A_bx < 4000.0*cos(30) l_long*sin30, # A_by < 4000.0*sin(30) 0, # A_cx < 0 l_long*sin30, # A_cy < 4000.0*sin(30) l_long, # A_dz < 4000.0 0.1, # x0 < 0.1 0.1, # y0 < 0.1 0.1, # z0 < 0.1 ] + [l_short, l_short, 2*l_short]*(u-1) from mystic.termination import ChangeOverGeneration, NormalizedChangeOverGeneration, VTR ... ... @@ -222,11 +218,11 @@ def solve(samp, cb, _cost = cost_sq, az = 0., bz = 0., cz = 0.): #pos_est0 = np.random.rand(u,3)*l_short - [l_short/2, l_short/2, 0] #pos_est0 = positions(5*5*5, 0, fuzz = 0) pos_est0 = np.zeros((u,3)) #anchors_est = symmetric_anchors(l_anch) #anchors_est = symmetric_anchors(l_anch, az, bz, cz) anchors_est = np.array([[0.0, 0.0, az], [0.0, 0.0, bz], [0.0, 0.0, cz], [0.0, 0.0, 0]]) [0.0, 0.0, 0.0]]) x_guess0 = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est0)) from mystic.termination import Or, CollapseAt, CollapseAs ... ... @@ -258,250 +254,72 @@ def solve(samp, cb, _cost = cost_sq, az = 0., bz = 0., cz = 0.): if __name__ == "__main__": # Gotten from manual measuring anchors = np.array([[0.0, -1675.0, 0.0], [-1290.0, 1450.0, 0.0], [1330.0, 1400.0, 0.0], [0.0, 0.0, 1900.0]]) # The last and final data collection samp = np.array([ [0.00, 0.00, 0.00, 0.00], [-18.48, 545.58, 150.52, -739.12], [17.11, 643.61, 310.02, -902.75], [82.10, 295.06, 578.13, -930.10], [416.71, -30.98, 598.49, -909.81], [398.76, 129.05, 276.42, -1026.36], [580.15, -145.47, -201.86, -571.18], [822.88, -119.11, -189.72, -689.79], [878.81, 92.06, -241.07, -775.01], [890.11, 30.04, -197.74, -780.53], [751.27, -211.24, 110.05, -814.89], [653.38, -216.52, 194.84, -817.91], [513.43, -102.17, 184.61, -855.04], [49.59, -72.44, 464.47, -477.06], [-1501.57, 1347.78, 1215.70, 531.90], [-1547.40, 1389.66, 1259.09, 567.85], [1515.69, -1658.76, 598.58, 626.25], [1377.30, -1387.86, 547.61, 267.91], [931.55, -824.87, 271.78, -258.19], [747.75, -446.98, 220.77, -624.25], [535.42, -115.40, 116.39, -821.45], [1078.13, 327.34, -1297.26, 401.30], [1083.88, 410.36, -1181.46, 113.76], [744.47, 215.68, -732.59, -280.05], [428.15, 69.37, -81.03, -767.44], [715.93, 575.91, 646.29, -1659.38], ]) # Use all the points # samp = np.array([ #[0.00, 0.00, 0.00, 0.00], #[-170.05, 152.38, 137.88, -179.97], #[-314.59, 304.17, 276.27, -267.88], #[-452.23, 437.76, 309.98, -145.62], #[-485.80, 338.49, 643.30, -141.04], #[-579.14, 443.48, 643.13, -119.91], #[-487.91, 476.20, 592.15, -337.04], #[-319.81, 433.71, 432.97, -502.55], #[-194.65, 404.48, 359.65, -617.36], #[101.11, 244.73, 237.07, -801.82], #[312.27, 129.08, 89.20, -828.35], #[451.16, -36.50, -60.20, -691.99], #[552.92, -53.49, -270.62, -548.23], #[633.16, -69.08, -305.83, -546.69], #[648.55, -152.04, -416.74, -351.97], #[716.93, -308.74, -429.31, -185.34], #[745.06, -496.04, -289.73, -108.97], #[804.04, -394.94, -506.72, 98.59], #[803.65, -146.07, -737.17, 120.83], #[767.01, -252.29, -610.04, 60.58], ##[777.01, -242.29, -612.04, 60.58], # Corrected: A: +10, B-10, C-2 #[559.23, -708.94, 464.54, -85.73], ## This one at mark ##[570.23, -720.94, 460.54, -85.73], ## corrected at mark: A: +11, B: -12, C: -4 #[772.27, -877.16, 203.65, 33.78], ##[782.27, -887.16, 201.65, 33.78], # Corrected: A: +10, B-10, C-2 #[582.82, -610.76, -15.97, 0.67], # #[260.97, 193.43, 223.06, -915.84], #[764.20, -5.79, 25.35, -915.84], #[766.09, -6.06, 25.35, -915.84], #[349.73, 33.83, 546.69, -915.84], #[139.19, 548.54, 190.50, -915.84], #[379.42, 292.80, 8.90, -915.84], #[313.46, 107.93, -294.35, -451.37], #[653.79, 85.09, -516.36, -451.37], #[815.93, -592.42, 155.61, -451.37], #[807.74, -592.89, 162.96, -451.37], #[347.51, -349.48, 469.19, -451.37], #[-265.20, 266.39, 528.10, -451.37], #[-759.54, 657.01, 664.33, -61.40], #[-762.04, 659.33, 668.54, -61.40], #[-804.31, 892.10, 589.58, -61.40], #[-334.78, 568.93, 50.68, -61.40], #[2.52, -1.66, 1.61, -2.57], #[689.62, -238.24, -563.38, -0.57], # #[260.97, 193.43, 223.06, -915.84], #[764.20, -5.79, 25.35, -915.84], #[766.09, -6.06, 25.35, -915.84], #[349.73, 33.83, 546.69, -915.84], #[139.19, 548.54, 190.50, -915.84], #[379.42, 292.80, 8.90, -915.84], #[313.46, 107.93, -294.35, -451.37], #[653.79, 85.09, -516.36, -451.37], #[815.93, -592.42, 155.61, -451.37], #[807.74, -592.89, 162.96, -451.37], #[347.51, -349.48, 469.19, -451.37], #[-265.20, 266.39, 528.10, -451.37], #[-759.54, 657.01, 664.33, -61.40], #[-762.04, 659.33, 668.54, -61.40], #[-804.31, 892.10, 589.58, -61.40], #[-334.78, 568.93, 50.68, -61.40], #[2.52, -1.66, 1.61, -2.57], #[691.62, -239.24, -562.38, -2.57], # ]) anchors = np.array([[0.0, -1112.0, -120.0], [970.0, 550.0, -120.0], [-970.0, 550.0, -120.0], [0.0, 0.0, 2865.0]]) # samp = np.array([ #[276.94, 53.00, 65.10, -737.12], #[285.35, 60.23, 92.42, -771.43], #[293.33, 66.82, 113.66, -798.70], #[305.34, 78.91, 142.21, -838.42], #[309.79, 112.75, 177.17, -891.07], #[331.22, 117.31, 185.32, -917.18], #[347.79, 124.86, 186.45, -937.50], #[355.02, 130.68, 187.74, -948.36], #[364.99, 140.88, 191.88, -965.49], #[374.09, 146.57, 195.26, -978.68], #[378.99, 152.48, 195.90, -987.22], #[379.85, 156.19, 198.20, -991.83], #[380.49, 161.54, 203.07, -999.10], #[380.71, 162.02, 203.03, -999.53], #[380.71, 162.23, 203.15, -999.81], #[380.71, 162.23, 203.36, -999.94], #]) ## # Combination one, converges # samp = np.array([ #[0.00, 0.00, 0.00, 0.00], #[276.94, 53.00, 65.10, -737.12], #[285.35, 60.23, 92.42, -771.43], #[293.33, 66.82, 113.66, -798.70], #[305.34, 78.91, 142.21, -838.42], #[309.79, 112.75, 177.17, -891.07], #[331.22, 117.31, 185.32, -917.18], #[347.79, 124.86, 186.45, -937.50], #[355.02, 130.68, 187.74, -948.36], #[364.99, 140.88, 191.88, -965.49], #[374.09, 146.57, 195.26, -978.68], #[378.99, 152.48, 195.90, -987.22], #[379.85, 156.19, 198.20, -991.83], #[380.49, 161.54, 203.07, -999.10], #[380.71, 162.02, 203.03, -999.53], #[380.71, 162.23, 203.15, -999.81], #[380.71, 162.23, 203.36, -999.94], #[260.97, 193.43, 223.06, -915.84], # D is off #[764.20, -5.79, 25.35, -915.84], # D is off #[766.09, -6.06, 25.35, -915.84], # D is off #[349.73, 33.83, 546.69, -915.84], # D is off ##[260.97, 193.43, 223.06, -905.84], # corrected D +10 ##[764.20, -5.79, 25.35, -905.84], # corrected D +10 ##[766.09, -6.06, 25.35, -905.84], # corrected D +10 ##[349.73, 33.83, 546.69, -905.84], # corrected D +10 #[-170.05, 152.38, 137.88, -179.97], #last #[-314.59, 304.17, 276.27, -267.88], #[-452.23, 437.76, 309.98, -145.62], #[-485.80, 338.49, 643.30, -141.04], #4 ## ##[367.86, 163.80, 223.92, -997.07], ## ###[777.01, -242.29, -612.04, 60.58], # Corrected: A: +10, B-10, C-2 ###[565.23, -715.94, 462.54, -85.73], ## corrected .. ###[782.27, -887.16, 201.65, 33.78], # Corrected: A: +10, B-10, C-2 #[126.31 , 5.02 , -0.21 , -213.52], #[295.03 , -257.68 , 218.73 , -244.16], #[511.65 , 94.13 , 116.17 , -585.52], #[373.57 , 615.00 , -132.03 , -570.93], #[285.95 , 468.10 , -475.99 , -112.57], #[411.75 , -471.95 , 279.45 , -61.84], #[646.11 , 257.49 , 289.34 , -845.42], #[43.83 , 384.27 , 262.25 , -618.82], #[-416.94 , 392.71 , 305.03 , -178.76], #[-355.53 , 308.31 , 408.93 , -267.15], #[191.34 , 555.78 , 209.78 , -741.28], #[537.90 , 574.98 , 470.11 , -1102.07], #[636.51 , 380.17 , 709.07 , -1118.74], #[897.10 , 913.95 , 702.54 , -1473.05] #]) # samp = np.array([ #[0.00, 0.00, 0.00, 0.00], #[-170.05, 152.38, 137.88, -179.97], #[-314.59, 304.17, 276.27, -267.88], #[-452.23, 437.76, 309.98, -145.62], #[-485.80, 338.49, 643.30, -141.04], #[-579.14, 443.48, 643.13, -119.91], #[-487.91, 476.20, 592.15, -337.04], #[-319.81, 433.71, 432.97, -502.55], #[-194.65, 404.48, 359.65, -617.36], #[101.11, 244.73, 237.07, -801.82], #[312.27, 129.08, 89.20, -828.35], #[451.16, -36.50, -60.20, -691.99], #[552.92, -53.49, -270.62, -548.23], #[633.16, -69.08, -305.83, -546.69], #[648.55, -152.04, -416.74, -351.97], #[716.93, -308.74, -429.31, -185.34], #[745.06, -496.04, -289.73, -108.97], #[804.04, -394.94, -506.72, 98.59], #[803.65, -146.07, -737.17, 120.83], #[767.01, -252.29, -610.04, 60.58], #[777.01, -242.29, -612.04, 60.58], # Corrected: A: +10, B-10, C-2 ##[559.23, -708.94, 464.54, -85.73], ## This one at mark #[570.23, -720.94, 460.54, -85.73], ## corrected at mark: A: +11, B: -12, C: -4 ##[772.27, -877.16, 203.65, 33.78], #[782.27, -887.16, 201.65, 33.78], # Corrected: A: +10, B-10, C-2 #[582.82, -610.76, -15.97, 0.67], #]) # Adapted to new zero # samp = np.array([ #[0.00, 0.00, 0.00, 0.00], #[260.97, 193.43, 223.06, -915.84], #[764.20, -5.79, 25.35, -915.84], #[766.09, -6.06, 25.35, -915.84], #[349.73, 33.83, 546.69, -915.84], #[139.19, 548.54, 190.50, -915.84], #[379.42, 292.80, 8.90, -915.84], #[313.46, 107.93, -294.35, -451.37], #[653.79, 85.09, -516.36, -451.37], #[815.93, -592.42, 155.61, -451.37], #[807.74, -592.89, 162.96, -451.37], #[347.51, -349.48, 469.19, -451.37], #[-265.20, 266.39, 528.10, -451.37], #[-759.54, 657.01, 664.33, -61.40], #[-762.04, 659.33, 668.54, -61.40], #[-804.31, 892.10, 589.58, -61.40], #[-334.78, 568.93, 50.68, -61.40], #[2.52, -1.66, 1.61, -2.57], #[689.62, -238.24, -563.38, -0.57], # ]) # samp = np.array([ #[0.00, 0.00, 0.00, 0.00], #[260.97, 193.43, 223.06, -915.84], #[764.20, -5.79, 25.35, -915.84], #[766.09, -6.06, 25.35, -915.84], #[349.73, 33.83, 546.69, -915.84], #[139.19, 548.54, 190.50, -915.84], #[379.42, 292.80, 8.90, -915.84], #[313.46, 107.93, -294.35, -451.37], #[653.79, 85.09, -516.36, -451.37], #[815.93, -592.42, 155.61, -451.37], #[807.74, -592.89, 162.96, -451.37], #[347.51, -349.48, 469.19, -451.37], #[-265.20, 266.39, 528.10, -451.37], #[-759.54, 657.01, 664.33, -61.40], #[-762.04, 659.33, 668.54, -61.40], #[-804.31, 892.10, 589.58, -61.40], #[-334.78, 568.93, 50.68, -61.40], #[2.52, -1.66, 1.61, -2.57], #[691.62, -239.24, -562.38, -2.57], #[400.53 , 175.53 , 166.10 , -656.90], #[229.27 , 511.14 , -48.41 , -554.31], #[-41.69 , -62.87 , 306.76 , -225.31], #[272.97 , 176.65 , 381.13 , -717.81], #[338.07 , 633.70 , 309.27 , -911.22], #[504.47 , 658.88 , 48.60 , -794.42], #[504.47 , 658.88 , 48.60 , -794.42], #[103.50 , 569.98 , 633.68 , -860.25], #[229.37 , 7.32 , 411.98 , -575.81], #[428.73 , -413.46 , 250.38 , -133.93], #[-506.97 , 343.33 , 327.68 , -4.40] # ]) samp = np.array([ [0.00, 0.00, 0.00, 0.00], [400.53 , 175.53 , 166.10 , -656.90], [229.27 , 511.14 , -48.41 , -554.31], [-41.69 , -62.87 , 306.76 , -225.31], [272.97 , 176.65 , 381.13 , -717.81], [338.07 , 633.70 , 309.27 , -911.22], [504.47 , 658.88 , 48.60 , -794.42], [504.47 , 658.88 , 48.60 , -794.42], [103.50 , 569.98 , 633.68 , -860.25], [229.37 , 7.32 , 411.98 , -575.81], [428.73 , -413.46 , 250.38 , -133.93], [-506.97 , 343.33 , 327.68 , -4.40], [126.31 , 5.02 , -0.21 , -213.52], [295.03 , -257.68 , 218.73 , -244.16], [511.65 , 94.13 , 116.17 , -585.52], [373.57 , 615.00 , -132.03 , -570.93], [285.95 , 468.10 , -475.99 , -112.57], [411.75 , -471.95 , 279.45 , -61.84], [646.11 , 257.49 , 289.34 , -845.42], [43.83 , 384.27 , 262.25 , -618.82], [-416.94 , 392.71 , 305.03 , -178.76], [-355.53 , 308.31 , 408.93 , -267.15], [191.34 , 555.78 , 209.78 , -741.28], [537.90 , 574.98 , 470.11 , -1102.07], [636.51 , 380.17 , 709.07 , -1118.74], [897.10 , 913.95 , 702.54 , -1473.05] ]) u = np.shape(samp)[0] pos = np.zeros((u, 3)) ... ... @@ -565,29 +383,40 @@ if __name__ == "__main__": #az = 0. #bz = 0. #cz = 0. az = -5. bz = 2.5 cz = 2.5 #for az in np.arange(-5.,5.,2.5): # for bz in np.arange(-5.,5.,2.5): # for cz in np.arange(-5.,5.,2.5): az = -110. bz = -110. cz = -110. #for az in np.arange(-115.,-125.1,-5.): # for bz in np.arange(-115.,-125.1,-5.): # for cz in np.arange(-115.,-125.1,-5.): # solution = solve(samp, mute, cost_sq, az, bz, cz) # sol_anch = anchorsvec2matrix(solution[0:6], az, bz, cz) # print("Output Anchors were: ") # print(sol_anch) # print("Anchor errors were: ") # print(sol_anch - anchors) # #print("Positions were: ") # #print(posvec2matrix(solution[6:], u)) # the_cost = cost_sq(anchorsvec2matrix(solution[0:6], az, bz, cz), np.reshape(solution[6:], (u,3)), samp) # print("cost: %f" % the_cost) # if(the_cost < best_cost): # best_cost = the_cost # best_az = az # best_bz = bz # best_cz = cz # print("Best az: %f\nBest bz: %f\nBest cz: %f\nBest cost: %f" % (best_az, best_bz, best_cz, best_cost)) solution = solve(samp, mute, cost_sq, az, bz, cz) sol_anch = anchorsvec2matrix(solution[0:6], az, bz, cz) print("Output Anchors were: ") print(sol_anch) print("Anchor errors were: ") print(sol_anch - anchors) #print("Positions were: ") #print(posvec2matrix(solution[6:], u)) the_cost = cost_sq(anchorsvec2matrix(solution[0:6], az, bz, cz), np.reshape(solution[6:], (u,3)), samp) print("cost: %f" % the_cost) #if(the_cost < best_cost): # best_cost = the_cost # best_az = az # best_bz = bz # best_cz = cz #print("Best az: %f\nBest bz: %f\nBest cz: %f\nBest cost: %f" % (best_az, best_bz, best_cz, best_cost)) print("cost found: %f" % the_cost) print("Anchors:") print(anchors) print("Error:") print(sol_anch-anchors) print("Found anchors:") print(sol_anch)
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!