bundletoa.py 文件源码

def bundletoa(*args):
    debug = True

    xt = None
    yt = None
    res = None
    jac = None

    for kkk in range(0, 30):

        D = args[0]
        i = args[1]
        J = args[2]
        xt = args[3]
        yt = args[4]
        res, jac = calcresandjac(D, i, J, xt, yt)

        dz_a = -((jac.conj().T) * jac + np.eye(jac.shape[1]))
        dz_b = (jac.conj().T) * res
        dz = linalg.lstsq(dz_a, dz_b)

        xtn, ytn = updatexy(xt, yt, dz)
        res2, jac2 = calcresandjac(D, i, J, xtn, ytn)

        cc = np.linalg.norm(jac * dz) / np.linalg.norm(res)

        if np.linalg.norm(res) < np.linalg.norm(res2):

            if cc > 1e-4:

                kkkk = 1
                while (kkkk < 50) and (
                        np.linalg.norm(res) < np.linalg.norm(res2)):
                    dz = dz / 2
                    xtn, ytn = updatexy(xt, yt, dz)
                    res2, jac2 = calcresandjac(D, i, J, xtn, ytn)
                    kkkk = kkkk + 1

        if debug:
            aa = np.concatenate((np.linalg.norm(res), np.linalg.norm(
                res + jac * dz), np.linalg.norm(res)), 1)
            bb = aa
            bb = bb - bb[1]
            bb = bb / bb[0]
            cc = (np.linalg.norm(jac * dz)) / np.linalg.norm(res)

            print(aa, bb, cc)

        if np.linalg.norm(res2) < np.linalg.norm(res):

            xt = xtn
            yt = ytn
        else:
            print()

    xopt = xt
    yopt = yt

    return xopt, yopt, res, jac