Commit 78a8417b by Torbjørn Ludvigsen 👷

### Plotting simulation action

parent 763dbd72
 ... ... @@ -3,8 +3,8 @@ from __future__ import division # Always want 3/2 = 1.5 from itertools import product import numpy as np import matplotlib.pyplot as plt import scipy #import mystic #import scipy # Axes indexing A = 0 ... ... @@ -108,14 +108,14 @@ def cost(anchors, pos, samp): This is the systems of equations: sum for i from 1 to u sum for k from a to d |sqrt(sum for s from x to z (A_sk-s_i)^2) - sqrt(sum for s from x to z (A_sk-s_0)^2) - t_ik| |sqrt(sum for s from x to z (A_ks-s_i)^2) - sqrt(sum for s from x to z (A_ks-s_0)^2) - t_ik| or... sum for i from 1 to u |sqrt((A_xa-x_i)^2 + (A_ya-y_i)^2 + (A_za-z_i)^2) - sqrt((A_xa-x_0)^2 + (A_ya-y_0)^2 + (A_za-z_0)^2) - t_ib| + |sqrt((A_xb-x_i)^2 + (A_yb-y_i)^2 + (A_zb-z_i)^2) - sqrt((A_xb-x_0)^2 + (A_yb-y_0)^2 + (A_zb-z_0)^2) - t_ib| + |sqrt((A_xc-x_i)^2 + (A_yc-y_i)^2 + (A_zc-z_i)^2) - sqrt((A_xc-x_0)^2 + (A_yc-y_0)^2 + (A_zc-z_0)^2) - t_ic| + |sqrt((A_xd-x_i)^2 + (A_yd-y_i)^2 + (A_zd-z_i)^2) - sqrt((A_xd-x_0)^2 + (A_yd-y_0)^2 + (A_zd-z_0)^2) - t_id| |sqrt((A_ax-x_i)^2 + (A_ay-y_i)^2 + (A_az-z_i)^2) - sqrt((A_ax-x_0)^2 + (A_ay-y_0)^2 + (A_az-z_0)^2) - t_ib| + |sqrt((A_bx-x_i)^2 + (A_by-y_i)^2 + (A_bz-z_i)^2) - sqrt((A_bx-x_0)^2 + (A_by-y_0)^2 + (A_bz-z_0)^2) - t_ib| + |sqrt((A_cx-x_i)^2 + (A_cy-y_i)^2 + (A_cz-z_i)^2) - sqrt((A_cx-x_0)^2 + (A_cy-y_0)^2 + (A_cz-z_0)^2) - t_ic| + |sqrt((A_dx-x_i)^2 + (A_dy-y_i)^2 + (A_dz-z_i)^2) - sqrt((A_dx-x_0)^2 + (A_dy-y_0)^2 + (A_dz-z_0)^2) - t_id| Parameters --------- ... ... @@ -124,28 +124,132 @@ def cost(anchors, pos, samp): samp : ux4 matrix of corresponding samples, starting with [0., 0., 0., 0.] """ return np.sum(np.abs(samples(anchors, pos, fuzz = 0) - samp)) #return np.sum((samples(anchors, pos, fuzz = 0) - samp) * (samples(anchors, pos, fuzz = 0) - samp)) # Sum of squares def anchorsvec2matrix(anchorsvec): """ Create a 4x3 anchors matrix from 6 element anchors vector. """ anchors = np.array(np.zeros((4, 3))) anchors[A,Y] = anchorsvec[0]; anchors[B,X] = anchorsvec[1]; anchors[B,Y] = anchorsvec[2]; anchors[C,X] = anchorsvec[3]; anchors[C,Y] = anchorsvec[4]; anchors[D,Z] = anchorsvec[5]; return anchors def anchorsmatrix2vec(a): return [a[A,Y], a[B, X], a[B,Y], a[C, X], a[C, Y], a[D, Z]] def posvec2matrix(v, u): return np.reshape(v, (u,3)) def posmatrix2vec(m): return np.reshape(m, np.shape(m)[0]*3) def solve(samp, cb): """Find reasonable positions and anchors given a set of samples. """ u = np.shape(samp)[0] cos30 = np.cos(30*np.pi/180) sin30 = np.sin(30*np.pi/180) number_of_params_pos = 3*u number_of_params_anch = 6 anchors_est = symmetric_anchors(1500.0) pos_est = [[0, 0, 500]]*u x_guess = list(anchorsmatrix2vec(anchors_est)) + list(posmatrix2vec(pos_est)) def costx(posvec, anchvec): """Identical to cost, except the shape of inputs and capture of samp and u Parameters ---------- 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) pos = np.reshape(posvec, (u,3)) return cost(anchors, pos, samp) from mystic.termination import ChangeOverGeneration from mystic.solvers import DifferentialEvolutionSolver2 # Bootstrap to give get x positons to cover some volume NP = 100 # Guessed size of the trial solution population pos_solver = DifferentialEvolutionSolver2(number_of_params_pos, NP) pos_solver.SetInitialPoints(x_guess[6:]) pos_solver.SetEvaluationLimits(generations=1500) pos_solver.Solve(lambda x: costx(x, list(anchorsmatrix2vec(anchors_est))), \ termination = ChangeOverGeneration(generations=300, tolerance=2), \ callback = cb) x_guess[6:] = pos_solver.bestSolution # Main solver solver = DifferentialEvolutionSolver2(number_of_params_pos+number_of_params_anch, NP) solver.SetInitialPoints(x_guess) solver.SetEvaluationLimits(generations=5000) solver.Solve(lambda x: costx(x[6:], x[0:6]), \ termination = ChangeOverGeneration(generations=300), \ callback = cb) return anchorsvec2matrix(solver.bestSolution[0:6]) # Do basic testing and output results # if this is run like a script # and not imported like a module or package if __name__ == "__main__": l_long = 1500 l_short = 150 n = 5 sample_fuzz = 5 l_long = 2000 l_short = 1000 n = 3 anchors = irregular_anchors(l_long) pos = positions(n, l_short, fuzz = 5) u = np.shape(pos)[0] # Plot out real position and anchor import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D plt.ion() # Make anchor figure and position figure. # Put the right answers onto those figures fig_anch = plt.figure() fig_pos = plt.figure() 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+') 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+') iter = 0 def replot(x): """Call while pos solver is running. """ global iter, u if iter%30 == 0: 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]) scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2]) print("Anchor errors: ") print(anchorsvec2matrix(x[0:6]) - anchors) elif len(x) == 6: anch = anchorsvec2matrix(x[0:6]) scat_anch._offsets3d = (anch[:,0], anch[:,1], anch[:,2]) else: ps = posvec2matrix(x, u) scat_pos._offsets3d = (ps[:,0], ps[:,1], ps[:,2]) plt.draw() plt.pause(0.1) iter += 1 sample_fuzz = 0 samp = samples(anchors, pos, fuzz = sample_fuzz) cost = cost(anchors, pos, samp) print("Cost was %f" % cost) eps = 3*(n**3)*sample_fuzz # Linear effect of fuzz seems about right print("eps was %f" % eps) if cost > eps: print("Test fail") else: print("Test success") # TODO: # * mystic solution = solve(samp, replot) print("Anchor errors were: ") print(solution - anchors) print("Cost were: %f" % cost(solution, pos, samp))
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