Commit 2e71b4d2 authored by Torbjørn Ludvigsen's avatar 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!
Please register or to comment