Python matrix函数代码示例

def robot_F_fn(state, dt = 1.0):
    """
    Transition matrix for robot dynamics

    Linearize dynamics about 'state' for EKF

    xdot = v*cos(theta+w)
    ydot = v*sin(theta+w)
    thetadot = w
    vdot = 0    -> step size
    wdot = 0    -> turn angle per step
    """
    x = state.value[0][0]
    y = state.value[1][0]
    theta = state.value[2][0]
    v = state.value[3][0]
    w = state.value[4][0]

    J = matrix([[0., 0.,     -v*sin(theta), cos(theta),              0.],
                [0., 0.,      v*cos(theta), sin(theta),              0.],
                [0., 0.,              0.,           0.,              1.],
                [0., 0.,              0.,           0.,              0.],
                [0., 0.,              0.,           0.,              0.],
               ])

    I = matrix([[]])
    I.identity(5)
    return I + J*dt

def estimate_next_pos(measurement, OTHER = None):
    """Estimate the next (x, y) position of the wandering Traxbot
    based on noisy (x, y) measurements."""
    # You must return xy_estimate (x, y), and OTHER (even if it is None) 
    # in this order for grading purposes.
    
    if OTHER == None:
        xy_estimate = measurement
        OTHER = [measurement]
    else :
        
        if len(OTHER) > 1:
            m = len(OTHER) - 1
            v1 = [OTHER[m][0] - OTHER[m-1][0],  OTHER[m][1] - OTHER[m-1][1]]
            v2 = [measurement[0] - OTHER[m][0], measurement[1] - OTHER[m][1]]
            dist = distance_between(measurement, OTHER[m])
            radian = btwAngle(v1, v2)
            sn = sin(radian)
            cs = cos(radian)

            r = matrix([[cs, -sn], [sn, cs]])
            X = matrix([[v2[0]], [v2[1]]])
            X = r * X

            x = X.value[0][0]
            y = X.value[1][0]
            v = unitVector([x, y])

            xy_estimate = [ measurement[0] + v[0] * dist, measurement[1] + v[1] * dist ]
        else:
            xy_estimate = measurement
        OTHER.append(measurement)

    return xy_estimate, OTHER 

def slam(data, N, num_landmarks, motion_noise, measurement_noise):

    # Set the dimension of the filter
    dim = 2 * (N + num_landmarks) 

    # make the constraint information matrix and vector
    Omega = matrix()
    Omega.zero(dim, dim)
    Omega.value[0][0] = 1.0
    Omega.value[1][1] = 1.0

    Xi = matrix()
    Xi.zero(dim, 1)
    Xi.value[0][0] = world_size / 2.0
    Xi.value[1][0] = world_size / 2.0
    
    # process the data

    for k in range(len(data)):

        # n is the index of the robot pose in the matrix/vector
        n = k * 2 
    
        measurement = data[k][0]
        motion      = data[k][1]
    
        # integrate the measurements
        for i in range(len(measurement)):
    
            # m is the index of the landmark coordinate in the matrix/vector
            m = 2 * (N + measurement[i][0])
    
            # update the information maxtrix/vector based on the measurement
            for b in range(2):
                Omega.value[n+b][n+b] +=  1.0 / measurement_noise
                Omega.value[m+b][m+b] +=  1.0 / measurement_noise
                Omega.value[n+b][m+b] += -1.0 / measurement_noise
                Omega.value[m+b][n+b] += -1.0 / measurement_noise
                Xi.value[n+b][0]      += -measurement[i][1+b] / measurement_noise
                Xi.value[m+b][0]      +=  measurement[i][1+b] / measurement_noise


        # update the information maxtrix/vector based on the robot motion
        for b in range(4):
            Omega.value[n+b][n+b] +=  1.0 / motion_noise
        for b in range(2):
            Omega.value[n+b  ][n+b+2] += -1.0 / motion_noise
            Omega.value[n+b+2][n+b  ] += -1.0 / motion_noise
            Xi.value[n+b  ][0]        += -motion[b] / motion_noise
            Xi.value[n+b+2][0]        +=  motion[b] / motion_noise

    # compute best estimate
    mu = Omega.inverse() * Xi

    # return the result
    return mu

def kalman_xy(x, P, measurement, R, motion = matrix([[0., 0., 0., 0.]]).transpose, Q = matrix([[1., 0.,0.,0.], [0., 1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]])):
    """
    Parameters:    
    x: initial state 4-tuple of location and velocity: (x0, x1, x0_dot, x1_dot)
    P: initial uncertainty convariance matrix
    measurement: observed position
    R: measurement noise 
    motion: external motion added to state vector x
    Q: motion noise (same shape as P)
    """
    return kalman(x, P, measurement, R, motion, Q, F = matrix([[1., 0., 1., 0.],[0., 1., 0., 1.],[0., 0., 1., 0.],[0., 0., 0., 1.]]), H = matrix([[1., 0., 0., 0.],[0., 1., 0., 0.]]))

def testcp(dim,verbose=False) :    # characteristic polynomial test
  X = matrix.Identity(dim,x);
  M = matrix(dim,dim,
             tuple(xrational(random()+random()*1j) for i in range(dim*dim)))
  cp = (M-X).det;
  try :
    if cp.denominator != 1 :
      print('Charpoly not a polynomial? Denominator = %s'
            %(cp.denominator.mapcoeffs(complex)));
      verbose=True;
    cp = cp.numerator;
  except :
    pass;    # was already polynomial
  Md = M.det;
  if verbose or cp(0) != Md or cp(M) :
    print(M.mapped(complex));
    print(cp.mapcoeffs(complex));
  if cp(0) != Md : # check that characteristic polynomial at 0 is determinant
    print('det %s != charpoly(0) %s'%(float(Md),float(cp(0))));
  cpM = cp(M);
  if cp(M) :    # check that matrix satisfies its characteristic polynomial
    print(M,cpM);
  MT = M.T
  cpMT = cp(MT);
  if cpMT :    # check that transposed matrix satisfies characteristic polynomial
    print(MT,cpMT);

    def update(s, measurement):
        H = matrix([[1.,0.,0.,0.], [0.,1.,0.,0.]]) # measurement function: reflect the fact that we observe x and y but not the two velocities
        R = matrix([[s.measurement_uncertainty,0.],[0.,s.measurement_uncertainty]]) # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
        I = matrix([[1.,0.,0.,0.],[0.,1.,0.,0.],[0.,0.,1.,0.],[0.,0.,0.,1.]]) # 4d identity matrix

        Z = matrix([list(measurement)])
        y = Z.transpose() - (H * s.x)
        S = H * s.P * H.transpose() + R
        K = s.P * H.transpose() * S.inverse()

        s.x = s.x + (K * y)
        s.P = (I - (K * H)) * s.P

def KalmanFilter(measurements):
    x = matrix([[measurements[0]], [0]]) # initial state (location and velocity)
    P = matrix([[10., 0.], [0., 1000.]]) # initial uncertainty

    predict_pos_x, predict_vol_x = filter(x, P, measurements)
    predict_pos_x = [item for sublist in predict_pos_x for item in sublist]
    predict_pos_x_int = [int(x) for x in predict_pos_x]
    print 'predict: '
    print predict_pos_x_int
    print 'reality: '
    print measurements
    print 'error:'
    print calculateError1D(predict_pos_x_int,measurements)

    def predict(s):
        dt = 1
        u = matrix([[0.], [0.], [0.], [0.]]) # external motion
        F = matrix([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # next state function: generalize the 2d version to 4d

        s.x = (F * s.x) + u
        s.P = F * s.P * F.transpose()

        # print(s.x)

        pos1 = s.x.value[0][0]
        pos2 = s.x.value[1][0]
        return (pos1, pos2)

    def h(self):
        # answers: what would be the next measurement according to current state?
        newx = matrix([[]])
        newx.zero(2,1)

        newx.value[0][0] = self.x.value[0][0]
        newx.value[1][0] = self.x.value[1][0]

        # jacobian
        self.H = matrix([
            [1., 0., 0., 0., 0.],
            [0., 1., 0., 0., 0.]
        ])
        return newx

    def predict(s):
        dt = 1
        u = matrix([[0.], [0.], [0.], [0.]]) # external motion
        # F needs to become the Jacobian df/dx
        F = matrix([[1., 0., dt, 0.], [0., 1., 0., dt], [0., 0., 1., 0.], [0., 0., 0., 1.]]) # next state function: generalize the 2d version to 4d
        Q = matrix([[0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.], [0., 0., 0., 0.]])

        s.x = s.f(s.x, u)     #(F * s.x) + u
        s.P = F * s.P * F.transpose() + Q

        # print(s.x)

        pos1 = s.x.value[0][0]
        pos2 = s.x.value[1][0]
        return (pos1, pos2)

def filter(x, P, measurements, quiet=True):
    predict_pos = []
    predict_vol = []
    for n in range(len(measurements)):
        # measurement update
        j = H * x
        y = matrix([[measurements[n]]])- j #y: 1*1
        a = H*P*H.transpose() #a: 1*1
        S = a + R #S: 1*1  ##a.inverse()
        z = P*H.transpose() #z: 2*1
        k = z*S.inverse() #k: 2*1
        b = k*y #b: 2*1
        x = x + b
        P = (I - k*H)*P
        # prediction
        x = F*x + u
        P = F*P*F.transpose()
        if not quiet:
            print 'x= '
            x.show()
            print 'P= '
            P.show()
        predict_pos.append(x.value[0])
        predict_vol.append(x.value[1])
    return predict_pos, predict_vol

def state_from_measurements(three_measurements):
    """
    Estimates state of robot from the last three measurements

    Assumes each movement of robot is a "step" and a "turn"
    Three measurements constitute two moves, from which turn angle, heading
    and step size can be inferred.
    """

    x1, y1 = three_measurements[-3]
    x2, y2 = three_measurements[-2]
    x3, y3 = three_measurements[-1]

    # Last two position vectors from measurements
    vec_1 = [x2 - x1, y2 - y1]
    vec_2 = [x3 - x2, y3 - y2]

    # Find last turning angle using dot product
    dot = sum(v1*v2 for v1,v2 in zip(vec_1, vec_2))
    mag_v1 = sqrt(sum(v**2 for v in vec_1))
    mag_v2 = sqrt(sum(v**2 for v in vec_2))

    v0 = mag_v2
    w0 = acos(dot/(mag_v1*mag_v2))
    if dot < 0:
        w0 = pi-w0
    theta0 = atan2(vec_2[1], vec_2[0]) + w0
    x0 = x3 + v0*cos(theta0 + w0)
    y0 = y3 + v0*sin(theta0 + w0)

    return matrix([[x3], [y3], [theta0], [v0], [w0]])

def extended_kalman_filter(z, x, u, P, F_fn, x_fn, H, R):
    """
    Applies extended kalman filter on system

    z -> measurement
    x -> last state
    u -> control vector
    P -> covariances
    F_fn -> Function that returns F matrix for given 'x'
    x_fn -> Updates 'x' using the non-linear derivatives
    H -> Measurement matrix
    R -> Measurement covariance
    """
    I = identity_matrix(x.dimx)
    # prediction
    F = F_fn(x)
    x = x_fn(x, u)
    P = F * P * F.transpose()

    # measurement update
    Z = matrix([z])
    y = Z.transpose() - (H * x)
    S = H * P * H.transpose() + R
    K = P * H.transpose() * S.inverse()
    x = x + (K * y)
    P = (I - (K * H)) * P

    return x, P

def kalman(x, P, measurement, R, motion, Q, F, H):
    '''
    Parameters:
    x: initial state
    P: initial uncertainty convariance matrix
    measurement: observed position (same shape as H*x)
    R: measurement noise (same shape as H)
    motion: external motion added to state vector x
    Q: motion noise (same shape as P)
    F: next state function: x_prime = F*x
    H: measurement function: position = H*x

    Return: the updated and predicted new values for (x, P)

    See also http://en.wikipedia.org/wiki/Kalman_filter

    This version of kalman can be applied to many different situations by
    appropriately defining F and H 
    '''
    # UPDATE x, P based on measurement m    
    # distance between measured and current position-belief
    y = matrix([measurement])
    y = y.transpose - H * x
    S = H * P * H.transpose + R  # residual convariance
    K = P * H.transpose * S.inverse    # Kalman gain
    x = x + K*y
    size = F.dimx
    I = matrix.identity(size) # identity matrix
    P = (I - K*H)*P

    # PREDICT x, P based on motion
    x = F*x + motion
    P = F*P*F.transpose + Q

    return x, P

    def g(self):
        distancediff = self.x.value[2][0]
        anglediff = self.x.value[3][0]
        theta = self.x.value[4][0] + anglediff

        x = self.x.value[0][0] + cos(theta) * distancediff
        y = self.x.value[1][0] + sin(theta) * distancediff

        # jacobian
        self.G = matrix([
            [1., 0., cos(self.x.value[4][0]), 0., -1 * sin(self.x.value[4][0]) * distancediff],
            [0., 1., sin(self.x.value[4][0]), 0., cos(self.x.value[4][0]) * distancediff],
            [0., 0., 1., 0., 0.],
            [0., 0., 0., 1., 0.],
            [0., 0., 0., 1., 1.]
        ])

        moved_angle = atan2(y - self.x.value[1][0], x - self.x.value[0][0])
        new_angle_diff = (moved_angle - self.x.value[4][0]) % (pi * 2)
        new_distance_diff = distance_between([x,y], [self.x.value[0][0],self.x.value[1][0]])

        self.x.value[0][0] = x
        self.x.value[1][0] = y
        self.x.value[2][0] = new_distance_diff
        self.x.value[3][0] = new_angle_diff
        self.x.value[4][0] = theta
        return self.x