Commit 46c94bfe authored by Torbjørn Ludvigsen's avatar Torbjørn Ludvigsen 👷

Exploring data set number 3, some plotting

parent b9d5aa5b
import matplotlib.pyplot as plt
# See where_to_move_during_calibration.py to find definition of diffs
plt.close("all")
plt.plot(diffs[:,2]) # Throw up (blue line) plot with C measurements data
plt.title('Expected C length minus measured C length')
plt.plot(diffs[:,2],'*') # Also plot some orange stars there
plt.tick_params( # Remove the x axis tics and numbering
axis='x', # changes apply to the x-axis
which='both', # both major and minor ticks are affected
bottom='off', # ticks along the bottom edge are off
top='off', # ticks along the top edge are off
labelbottom='off') # labels along the bottom edge are off
ax = plt.gca() # Grab an axis object
ax.set_ylabel('C/mm')
plt.plot(x, m*x + c, 'r', label='Fitted line')
plt.legend()
......@@ -128,17 +128,21 @@ def cost(anchors, pos, samp):
return np.sum(np.abs(samples(anchors, pos, fuzz = 0) - samp))
def cost_sq(anchors, pos, samp):
pos[0] = [0.0, 0.0, 0.0]
return np.sum(pow((samples(anchors, pos, fuzz = 0) - samp), 2)) # Sum of squares
def anchorsvec2matrix(anchorsvec):
def anchorsvec2matrix(anchorsvec, z = 0):
""" Create a 4x3 anchors matrix from 6 element anchors vector.
"""
anchors = np.array(np.zeros((4, 3)))
anchors[A,Y] = anchorsvec[0];
anchors[A,Z] = z;
anchors[B,X] = anchorsvec[1];
anchors[B,Y] = anchorsvec[2];
anchors[B,Z] = z;
anchors[C,X] = anchorsvec[3];
anchors[C,Y] = anchorsvec[4];
anchors[C,Z] = z;
anchors[D,Z] = anchorsvec[5];
return anchors
......@@ -151,7 +155,7 @@ def posvec2matrix(v, u):
def posmatrix2vec(m):
return np.reshape(m, np.shape(m)[0]*3)
def solve(samp, cb, _cost = cost_sq):
def solve(samp, cb, _cost = cost_sq, z = 0):
"""Find reasonable positions and anchors given a set of samples.
"""
def costx(posvec, anchvec):
......@@ -162,7 +166,7 @@ def solve(samp, cb, _cost = cost_sq):
x : [A_ay A_bx A_by A_cx A_cy A_dz
x1 y1 z1 x2 y2 z2 ... xu yu zu
"""
anchors = anchorsvec2matrix(anchvec)
anchors = anchorsvec2matrix(anchvec, z)
pos = np.reshape(posvec, (u,3))
return _cost(anchors, pos, samp)
......@@ -175,13 +179,12 @@ def solve(samp, cb, _cost = cost_sq):
sin30 = np.sin(30*np.pi/180)
number_of_params_pos = 3*u
number_of_params_anch = 6
anchors_est = symmetric_anchors(l_anch)
# Define bounds
lb = [ -l_long, # A_ay > -5000.0
0, # A_bx > 0
-l_long*cos30, # A_bx > -5000*cos(30)
0, # A_by > 0
-l_long*cos30, # A_cx > -5000*cos(30)
0, # A_cx > 0
0, # A_cy > 0
0, # A_dz > 0
-50.0, # x0 > -50.0
......@@ -189,11 +192,11 @@ def solve(samp, cb, _cost = cost_sq):
-10, # z0 > -10
] + [-l_short, -l_short, -10]*(u-1)
ub = [ 0, # A_ay < 0
l_long*cos30, # A_bx < 5000.0*cos(30)
0, # A_bx < 0
l_long*sin30, # A_by < 5000.0*sin(30)
0, # A_cx < 0
l_long*cos30, # A_cx < 5000.0*cos(30)
l_long*sin30, # A_cy < 5000.0*sin(30)
2*l_long, # A_dz < 10000.0
l_long, # A_dz < 10000.0
50.0, # x0 < 50.0
50.0, # y0 < 50.0
l_short, # z0 < l_short
......@@ -202,7 +205,19 @@ def solve(samp, cb, _cost = cost_sq):
from mystic.termination import ChangeOverGeneration, NormalizedChangeOverGeneration, VTR
from mystic.solvers import DifferentialEvolutionSolver2, PowellDirectionalSolver
pos_est0 = np.random.rand(u,3)*l_short - [l_short/2, l_short/2, 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 = np.array([[0.0, -2163.0, -75.5],
# [-1841.0, 741.0, -75.5],
# [1639.0, 1404.0, -75.5],
# [0.0, 0.0, 3250.5]])
anchors_est = np.array([[0.0, 0.0, z],
[0.0, 0.0, z],
[0.0, 0.0, z],
[0.0, 0.0, 0]])
x_guess0 = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est0))
print("Solver 0")
......@@ -216,21 +231,142 @@ def solve(samp, cb, _cost = cost_sq):
print("Solver %d" % i)
solver0 = PowellDirectionalSolver(number_of_params_pos+number_of_params_anch)
solver0.SetInitialPoints(x_guess0)
solver0.SetStrictRanges(lb, ub)
solver0.Solve(lambda x: costx(x[6:], x[0:6]), callback = cb)
x_guess0 = solver0.bestSolution
anch_0 = anchorsvec2matrix(solver0.bestSolution[0:6])
#anch_0 = anchorsvec2matrix(solver0.bestSolution[0:6], z)
return anch_0
#return anch_0
return solver0.bestSolution
if __name__ == "__main__":
l_long = 1500
l_short = 400
n = 7
pos_fuzz = 20
anchors = irregular_anchors(l_long, 0.5)
pos = positions(n, l_short, fuzz = pos_fuzz)
u = np.shape(pos)[0]
z = -75.5
# Gotten from manual measuring
anchors = np.array([[0.0, -2163.0, -75.5],
[-1841.0, 741.0, -75.5],
[1639.0, 1404.0, -75.5],
[0.0, 0.0, 3250.5]])
# Calculated, not measured
# Gives exact solution up to e-8 precision
#samp = np.array([[ 0. , 0. , 0. , 0. ],
# [-160.05450141, -7.41578381, 508.81184094, -566.25890613],
# [-240.31902526, -88.76779973, 449.05946137, -269.65276306],
# [-275.807475 , -124.76718348, 422.99492029, 27.57111729],
# [ 121.48001868, -147.47176573, 327.9627796 , -583.0761023 ],
# [-182.63383271, 234.22786546, 299.45677071, -583.0761023 ],
# [ 51.43751 , -235.32008868, 263.75505139, -284.78756451],
# [ 20.69285473, -274.3991353 , 235.67481709, 13.81466774],
# [-263.85173603, 162.04723258, 234.48460575, -284.78756451],
# [-299.78804098, 130.31868502, 206.05693659, 13.81466774],
# [ 407.1938119 , -246.58073095, 171.82074196, -566.25890613],
# [-160.05450141, 488.67753676, 110.5331805 , -566.25890613],
# [ 345.13869583, -339.709284 , 103.18644535, -269.65276306],
# [ 101.70771773, 110.37864749, 101.92650368, -600. ],
# [ 318.03428538, -381.3285948 , 73.08753928, 27.57111729],
# [-240.31902526, 424.12957408, 39.987215 , -269.65276306],
# [ 31.03446609, 33.77678835, 31.10338123, -300. ],
# [-275.807475 , 395.89476307, 9.01126285, 27.57111729],
# [ 389.63442186, 24.02428293, -71.04756549, -583.0761023 ],
# [ 121.48001868, 378.19573436, -105.08271722, -583.0761023 ],
# [ 327.14199005, -56.00201336, -147.95175501, -284.78756451],
# [ 299.8396334 , -91.37870646, -181.86930811, 13.81466774],
# [ 51.43751 , 310.5445596 , -183.31098627, -284.78756451],
# [ 20.69285473, 280.89543594, -217.84612468, 13.81466774],
# [ 407.1938119 , 301.97210143, -297.18630107, -566.25890613],
# [ 345.13869583, 231.99721017, -383.85809756, -269.65276306],
# [ 318.03428538, 201.28360206, -422.37578312, 27.57111729]])
# Measured3
samp = np.array([[ 0.0 , 0.0 , 0.0 , 0.0],
[ -159.88 , -7.42 , 498.96 , -566.56],
[ -240.01 , -88.81 , 449.46 , -269.41],
[ -275.44 , -124.74 , 427.20 , 27.56],
[ 121.50 , -147.40 , 316.38 , -583.50],
[ -182.43 , 234.08 , 303.25 , -583.50],
[ 51.45 , -235.10 , 261.35 , -284.52],
[ 20.70 , -274.11 , 234.92 , 13.81],
[ -263.50 , 162.03 , 238.27 , -284.52],
[ -299.37 , 130.35 , 212.02 , 13.81],
[ 406.84 , -246.34 , 163.49 , -566.57],
[ -159.88 , 488.26 , 124.25 , -566.56],
[ 344.89 , -339.30 , 104.47 , -269.41],
[ 101.72 , 110.40 , 101.92 , -600.54],
[ 317.83 , -380.85 , 72.50 , 27.56],
[ -240.01 , 423.67 , 58.79 , -269.41],
[ 31.05 , 33.78 , 35.93 , -299.71],
[ -275.44 , 395.48 , 26.22 , 27.56],
[ 389.31 , 24.03 , -75.26 , -583.50]])
# Measured 3 and compensated with constant -2
#samp = np.array([[ 0. , 0. , 0. , 0. ],
# [-159.88, -7.42, 496.96, -566.56],
# [-240.01, -88.81, 447.46, -269.41],
# [-275.44, -124.74, 425.2 , 27.56],
# [ 121.5 , -147.4 , 314.38, -583.5 ],
# [-182.43, 234.08, 301.25, -583.5 ],
# [ 51.45, -235.1 , 259.35, -284.52],
# [ 20.7 , -274.11, 232.92, 13.81],
# [-263.5 , 162.03, 236.27, -284.52],
# [-299.37, 130.35, 210.02, 13.81],
# [ 406.84, -246.34, 161.49, -566.57],
# [-159.88, 488.26, 122.25, -566.56],
# [ 344.89, -339.3 , 102.47, -269.41],
# [ 101.72, 110.4 , 99.92, -600.54],
# [ 317.83, -380.85, 70.5 , 27.56],
# [-240.01, 423.67, 56.79, -269.41],
# [ 31.05, 33.78, 33.93, -299.71],
# [-275.44, 395.48, 24.22, 27.56],
# [ 389.31, 24.03, -77.26, -583.5 ]])
#Compensating -2 mm out of every C measurement reduced errors from
#[[ 0. -170.74775148 0. ]
# [-229.31816957 150.55807494 0. ]
# [-260.75172296 -196.38194866 0. ]
# [ 0. 0. -373.52002034]]
# to
#[[ 0. -139.74915668 0. ]
# [-211.47544526 122.72584696 0. ]
# [-204.44639263 -140.1629383 0. ]
# [ 0. 0. -425.38964298]]
# All coordinates except D_z got better
# Measured 3 and compensated with -0.64x + 3.85
#samp = np.array([[ 0. , 0. , 0. , 0. ],
# [-159.88 , -7.42 , 502.1720535 , -566.56 ],
# [-240.01 , -88.81 , 452.03203292, -269.41 ],
# [-275.44 , -124.74 , 429.13201234, 27.56 ],
# [ 121.5 , -147.4 , 317.67199176, -583.5 ],
# [-182.43 , 234.08 , 303.90197117, -583.5 ],
# [ 51.45 , -235.1 , 261.36195059, -284.52 ],
# [ 20.7 , -274.11 , 234.29193001, 13.81 ],
# [-263.5 , 162.03 , 237.00190943, -284.52 ],
# [-299.37 , 130.35 , 210.11188885, 13.81 ],
# [ 406.84 , -246.34 , 160.94186826, -566.57 ],
# [-159.88 , 488.26 , 121.06184768, -566.56 ],
# [ 344.89 , -339.3 , 100.6418271 , -269.41 ],
# [ 101.72 , 110.4 , 97.45180652, -600.54 ],
# [ 317.83 , -380.85 , 67.39178594, 27.56 ],
# [-240.01 , 423.67 , 53.04176536, -269.41 ],
# [ 31.05 , 33.78 , 29.54174477, -299.71 ],
# [-275.44 , 395.48 , 19.19172419, 27.56 ],
# [ 389.31 , 24.03 , -82.92829639, -583.5 ]])
#Compensating the linear measurment error reduced anchor errors from
#[[ 0. -170.74775148 0. ]
# [-229.31816957 150.55807494 0. ]
# [-260.75172296 -196.38194866 0. ]
# [ 0. 0. -373.52002034]]
# to
#[[ 0. -81.27827747 0. ]
# [-197.95233601 127.92453157 0. ]
# [-120.13780415 -76.13893988 0. ]
# [ 0. 0. -600.36145758]]
# All coordinates except D_z got better. D_z got 22.5 cm worse.
u = np.shape(samp)[0]
pos = np.zeros((u, 3))
# Plot out real position and anchor
import matplotlib.pyplot as plt
......@@ -244,7 +380,7 @@ if __name__ == "__main__":
ax_anch = fig_anch.add_subplot(111, projection='3d')
ax_pos = fig_pos.add_subplot(111, projection='3d')
scat_anch0 = ax_anch.scatter(anchors[:,0], anchors[:,1], anchors[:,2], 'ro')
scat_pos0 = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'k+')
#scat_pos0 = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'k+')
plt.pause(1)
scat_anch = ax_anch.scatter(anchors[:,0], anchors[:,1], anchors[:,2], 'yx')
scat_pos = ax_pos.scatter(pos[:,0], pos[:,1], pos[:,2], 'b+')
......@@ -258,38 +394,31 @@ if __name__ == "__main__":
if len(x) == 6 + 3*u:
ps = posvec2matrix(x[6:], u)
scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2])
anch = anchorsvec2matrix(x[0:6])
anch = anchorsvec2matrix(x[0:6], z)
scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2])
print("Anchor errors: ")
print(anchorsvec2matrix(x[0:6]) - anchors)
print(anchorsvec2matrix(x[0:6], z) - anchors)
print("cost: %f" % \
cost(anchorsvec2matrix(x[0:6]), np.reshape(x[6:], (u,3)), samp))
cost(anchorsvec2matrix(x[0:6], z), np.reshape(x[6:], (u,3)), samp))
elif len(x) == 6:
anch = anchorsvec2matrix(x[0:6])
anch = anchorsvec2matrix(x[0:6], z)
scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2])
else:
ps = posvec2matrix(x, u)
scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2])
#scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2])
plt.draw()
plt.pause(0.001)
iter += 1
sample_fuzz = 2
samp = samples(anchors, pos, fuzz = sample_fuzz)
solution = solve(samp, replot)
print("Used number of calibration points (mm)")
print(n*n*n)
print("Movement length along each axis while sampling were ca (mm)")
print(l_short)
print("With and uncertainty of (mm)")
print(pos_fuzz)
print("Samples were fuzzed by (mm)")
print(sample_fuzz)
print("Exact Anchors were: ")
print(anchors)
solution = solve(samp, replot, z = -75.5)
sol_anch = anchorsvec2matrix(solution[0:6], z = -75.5)
print("Output Anchors were: ")
print(solution)
print(sol_anch)
print("Anchor errors were: ")
print(solution - anchors)
print("Real cost were: %f" % cost(solution, pos, samp))
print(sol_anch - anchors)
print("cost: %f" % \
cost(anchorsvec2matrix(solution[0:6], z), np.reshape(solution[6:], (u,3)), samp))
#print("Real cost were: %f" % cost(solution, pos, samp))
#plt.pause(2000)
from simulation import *
a = symmetric_anchors(1500)
pos = positions(3, 400, 0)
samp = samples(a, pos, 0)
samp = samp[samp[:,2].argsort()]
samp = samp[samp[:,3].argsort(kind='mergesort')]
samp_rel = np.zeros(np.shape(samp))
#a = symmetric_anchors(1500)
for i in range(1, 27):
samp_rel[i] = samp[i] - samp[i-1]
# From measurment
a = np.array([[0.0, -2163.0, -75.5],
[-1841.0, 741.0, -75.5],
[1639.0, 1404.0, -75.5],
[0.0, 0.0, 3250.5]])
last_movement = -samp[26] # Go back
pos = positions(3, 300, 0)
samp_exp = samples(a, pos, 0)
# Keep the zero measurement as the first row
samp_exp_view = samp_exp[1:]
# Make index 2 = C motor the sensor motor
samp_exp_view = samp_exp_view[samp_exp_view[:,1].argsort()]
samp_exp_view = samp_exp_view[samp_exp_view[:,2].argsort(kind='mergesort')]
samp_exp[1:] = samp_exp_view
# Array samp is gotten from measurements
u = np.shape(samp)[0]
diffs = samp_exp[0:u] - samp
x = np.arange(u)
A = np.vstack([x, np.ones(len(x))]).T # A = [[x 1]]
m, c = np.linalg.lstsq(A, diffs[:,2])[0] # Linear regression. diffs[:,2] ~= mx + c
# Results:
# np.mean(diffs[:,2]) = -1.9
# np.std(diffs[:,2]) = 7.95
# m = -0.64
# c = 3.85
# After removing linear error:
# np.mean(diffs[:,2]) = 0.2
# np.std(diffs[:,2]) = 7.1
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