observer.py 文件源码

def __init__(self, settings):
        settings.update(output_dim=4)
        super().__init__(settings)

        params = sp.symbols('x1, x2, x3, x4, tau')
        x1, x2, x3, x4, tau = params
        x = [x1, x2, x3, x4]
        h = sp.Matrix([[x1]])
        f = sp.Matrix([[x2],
                       [st.B * x1 * x4 ** 2 - st.B * st.G * sin(x3)],
                       [x4],
                       [(tau - 2 * st.M * x1 * x2 * x4
                         - st.M * st.G * x1 * cos(x3)) / (
                        st.M * x1 ** 2 + st.J + st.Jb)]])

        q = sp.Matrix(pm.lie_derivatives(h, f, x, len(x) - 1))
        dq = q.jacobian(x)

        if dq.rank() != len(x):
            raise Exception("System might not be observable")

        # gets p = [p0, p1, ... pn-1]
        p = pm.char_coefficients(self._settings["poles"])

        k = p[::-1]
        l = dq.inv() @ k

        mat2array = [{'ImmutableMatrix': np.array}, 'numpy']
        self.h_func = sp.lambdify((x1, x2, x3, x4, tau), h, modules=mat2array)
        self.l_func = sp.lambdify((x1, x2, x3, x4, tau), l, modules=mat2array)
        self.f_func = sp.lambdify((x1, x2, x3, x4, tau), f, modules=mat2array)

        self.output = np.array(self._settings["initial state"], dtype=float)