sonar/prototype2.py at main · firelilith/sonar · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
#!/bin/python3
import math
import random
import time

import numpy
import scipy.optimize

def error_func(mics, vals):
    x, y, r = vals
    total = 0
    for mic in mics:
        xm, ym, c = mic
        total += (
           (x-xm)**2 + (y-ym)**2 - (r + c)**2
        ) ** 2

    return total

def grad_err_func(mics, vals):
    x, y, r = vals
    dx, dy, dr = 0, 0, 0
    for mic in mics:
        xm, ym, c = mic

        dx += (
            4 * (x - xm) * (
                (x-xm)**2 + (y-ym)**2 - (r + c)**2
            )
        )
        dy += (
            4 * (y - ym) * (
                (x-xm)**2 + (y-ym)**2 - (r + c)**2
            )
        )
        dr += (
            -4 * (r + c) * (
                (x-xm)**2 + (y-ym)**2 - (r + c)**2
            )
        )

    return dx, dy, dr

def ooptimize(mics, d = 0.1):
    x, y, r = 0, 0, 0
    err_change = float("inf")
    err = error_func(mics, x, y, r)

    while abs(err_change) > 0.01:
        dx, dy, dr = grad_err_func(mics, x, y, r)
        nx = x - d * dx
        ny = y - d * dy
        nr = r - d * dr

        if err < error_func(mics, nx, ny, nr):
            d = d/2
            continue

        x = nx
        y = ny
        r = nr

        print(x, y, r, " -> ", dx, dy, dr)

        time.sleep(0.1)

    return x, y, r

def optimize(mics):
    x0 = 0, 0, 0
    out = scipy.optimize.minimize(lambda x: error_func(mics, x), x0, method="BFGS",
                                  jac = lambda x: grad_err_func(mics, x))
    return out.x


def noise_mics(mics, n=0.1):
    mics = [(i, j, k * (1 + (-0.5 + random.random()) * n)) for (i, j, k) in mics]
    min_r = min(mic[2] for mic in mics)
    mics = [(i, j, k - min_r) for (i, j, k) in mics]
    return mics


def get_r_phi(x, y, r):
    r = (x**2 + y**2)**0.5
    phi = math.atan2(x, y) / math.pi * 180

    return r, phi


if __name__=="__main__":
    mics = [
        (0, -1, 2**0.5),
        (1, 0, 2),
        (0, 1, 2**0.5)
    ]

    rs, phis = [], []

    for noise in range(10):

        for i in range(100):

            n_mics = noise_mics(mics, 0.02 * noise)
    #        print(n_mics)

            xs = optimize(n_mics)
    #        print(xs)
    #        print("r={:2f} phi={:2f}°".format(*get_r_phi(*xs)))
            r, phi = get_r_phi(*xs)
            rs.append(r)
            phis.append(phi)

        print(f"noise = {noise * 0.05} \nr = ({numpy.mean(rs):.2f}+-{numpy.std(rs):.2f}) \nphi = ({numpy.mean(phis):.2f}+-{numpy.std(phis):.2f})°\n")