def kalman_filter(kalman_state, old_indices, coordinates, q, r):
    '''Return the kalman filter for the features in the new frame

    kalman_state - state from last frame

    old_indices - the index per feature in the last frame or -1 for new

    coordinates - Coordinates of the features in the new frame.

    q - the process error covariance - see equ 1.3 and 1.10 from Welch

    r - measurement error covariance of features - see eqn 1.7 and 1.8 from welch.

    returns a new KalmanState containing the kalman filter of
    the last state by the given coordinates.

    Refer to kalmanGainLinearMotion.m and
    http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

    for info on the algorithm.
    '''
    assert isinstance(kalman_state, KalmanState)
    old_indices = np.array(old_indices)
    if len(old_indices) == 0:
        return KalmanState(kalman_state.observation_matrix,
                           kalman_state.translation_matrix)
    #
    # Cull missing features in old state and collect only matching coords
    #
    matching = old_indices != -1
    new_indices = np.arange(len(old_indices))[~matching]
    retained_indices = np.arange(len(old_indices))[matching]
    new_coords = coordinates[new_indices]
    observation_matrix_t = kalman_state.observation_matrix.transpose()
    if len(retained_indices) > 0:
        kalman_state = kalman_state.deep_copy()
        coordinates = coordinates[retained_indices]
        kalman_state.map_frames(old_indices[retained_indices])
        #
        # Time update equations
        #
        # From eqn 1.9 of Welch
        #
        state_vec = kalman_state.predicted_state_vec
        #
        # From eqn 1.10 of Welch
        #
        state_cov = dot_n(
            dot_n(kalman_state.translation_matrix, kalman_state.state_cov),
            kalman_state.translation_matrix.transpose()) + q[matching]
        #
        # From eqn 1.11 of welch
        #
        kalman_gain_numerator = dot_n(state_cov, observation_matrix_t)

        kalman_gain_denominator = dot_n(
            dot_n(kalman_state.observation_matrix, state_cov),
                  observation_matrix_t) + r[matching]
        kalman_gain_denominator = inv_n(kalman_gain_denominator)
        kalman_gain = dot_n(kalman_gain_numerator, kalman_gain_denominator)
        #
        # Eqn 1.12 of Welch
        #
        difference = coordinates - dot_n(kalman_state.observation_matrix,
                                         state_vec[:,:,np.newaxis])[:,:,0]
        state_noise = dot_n(kalman_gain, difference[:,:,np.newaxis])[:,:,0]
        state_vec = state_vec + state_noise
        #
        # Eqn 1.13 of Welch (factored from (I - KH)P to P - KHP)
        #
        state_cov = (state_cov -
                     dot_n(dot_n(kalman_gain, kalman_state.observation_matrix),
                           state_cov))
        #
        # Collect all of the state noise in one array. We produce an I and J
        # variance. Notes in kalmanGainLinearMotion indicate that you
        # might want a single variance, combining I & J. An alternate
        # might be R and theta, a variance of angular consistency and one
        # of absolute velocity.
        #
        # Add an index to the state noise in the rightmost column
        #
        idx = np.arange(len(state_noise))
        #
        # Stack the rows with the old ones
        #
        all_state_noise = np.vstack((kalman_state.state_noise, state_noise))
        all_state_noise_idx = np.hstack((kalman_state.state_noise_idx, idx))
        noise_var = np.zeros((len(idx), all_state_noise.shape[1]))
        for i in range(all_state_noise.shape[1]):
            noise_var[:, i] = fix(scind.variance(all_state_noise[:, i],
                                                 all_state_noise_idx,
                                                 idx))
        obs_vec = dot_n(kalman_state.observation_matrix,
                        state_vec[:,:,np.newaxis])[:,:,0]
        kalman_state = KalmanState(kalman_state.observation_matrix,
                                   kalman_state.translation_matrix,
                                   state_vec, state_cov, noise_var,
                                   all_state_noise,
                                   all_state_noise_idx)
    else:
        # Erase all previous features
        kalman_state = KalmanState(kalman_state.observation_matrix,
                                   kalman_state.translation_matrix)
    if len(new_coords) > 0:
        #
        # Fill in the initial states:
        #
        state_vec = dot_n(observation_matrix_t,
                          new_coords[:,:,np.newaxis])[:,:,0]
        #
        # The COV for the hidden, undetermined features should be large
        # and the COV for others should be small
        #
        nstates = kalman_state.state_len
        nnew_features = len(new_indices)
        cov_vec = SMALL_KALMAN_COV / np.dot(observation_matrix_t,
                                            np.ones(kalman_state.obs_len))
        cov_vec[~ np.isfinite(cov_vec)] = LARGE_KALMAN_COV
        cov_matrix = np.diag(cov_vec)
        state_cov = cov_matrix[np.newaxis,:,:][np.zeros(nnew_features,int)]
        #
        # The noise variance is all ones in Jaqman
        #
        noise_var = np.ones((len(new_indices), kalman_state.state_len))
        #
        # Map the retained indices to their new slots and new ones to empty
        # slots (=-1)
        #
        kalman_state.add_features(retained_indices,
                                  new_indices,
                                  state_vec, state_cov, noise_var)
    return kalman_state
def kalman_filter ( kalman_state , old_indices , coordinates , q , r ) : assert isinstance ( kalman_state , KalmanState ) old_indices = np . array ( old_indices ) if len ( old_indices ) == 0 : return KalmanState ( kalman_state . observation_matrix , kalman_state . translation_matrix ) matching = old_indices != - 1 new_indices = np . arange ( len ( old_indices ) ) [ ~ matching ] retained_indices = np . arange ( len ( old_indices ) ) [ matching ] new_coords = coordinates [ new_indices ] observation_matrix_t = kalman_state . observation_matrix . transpose ( ) if len ( retained_indices ) > 0 : kalman_state = kalman_state . deep_copy ( ) coordinates = coordinates [ retained_indices ] kalman_state . map_frames ( old_indices [ retained_indices ] ) state_vec = kalman_state . predicted_state_vec state_cov = dot_n ( dot_n ( kalman_state . translation_matrix , kalman_state . state_cov ) , kalman_state . translation_matrix . transpose ( ) ) + q [ matching ] kalman_gain_numerator = dot_n ( state_cov , observation_matrix_t ) kalman_gain_denominator = dot_n ( dot_n ( kalman_state . observation_matrix , state_cov ) , observation_matrix_t ) + r [ matching ] kalman_gain_denominator = inv_n ( kalman_gain_denominator ) kalman_gain = dot_n ( kalman_gain_numerator , kalman_gain_denominator ) difference = coordinates - dot_n ( kalman_state . observation_matrix , state_vec [ : , : , np . newaxis ] ) [ : , : , 0 ] state_noise = dot_n ( kalman_gain , difference [ : , : , np . newaxis ] ) [ : , : , 0 ] state_vec = state_vec + state_noise state_cov = ( state_cov - dot_n ( dot_n ( kalman_gain , kalman_state . observation_matrix ) , state_cov ) ) idx = np . arange ( len ( state_noise ) ) all_state_noise = np . vstack ( ( kalman_state . state_noise , state_noise ) ) all_state_noise_idx = np . hstack ( ( kalman_state . state_noise_idx , idx ) ) noise_var = np . zeros ( ( len ( idx ) , all_state_noise . shape [ 1 ] ) ) for i in range ( all_state_noise . shape [ 1 ] ) : noise_var [ : , i ] = fix ( scind . variance ( all_state_noise [ : , i ] , all_state_noise_idx , idx ) ) obs_vec = dot_n ( kalman_state . observation_matrix , state_vec [ : , : , np . newaxis ] ) [ : , : , 0 ] kalman_state = KalmanState ( kalman_state . observation_matrix , kalman_state . translation_matrix , state_vec , state_cov , noise_var , all_state_noise , all_state_noise_idx ) else : kalman_state = KalmanState ( kalman_state . observation_matrix , kalman_state . translation_matrix ) if len ( new_coords ) > 0 : state_vec = dot_n ( observation_matrix_t , new_coords [ : , : , np . newaxis ] ) [ : , : , 0 ] nstates = kalman_state . state_len nnew_features = len ( new_indices ) cov_vec = SMALL_KALMAN_COV / np . dot ( observation_matrix_t , np . ones ( kalman_state . obs_len ) ) cov_vec [ ~ np . isfinite ( cov_vec ) ] = LARGE_KALMAN_COV cov_matrix = np . diag ( cov_vec ) state_cov = cov_matrix [ np . newaxis , : , : ] [ np . zeros ( nnew_features , int ) ] noise_var = np . ones ( ( len ( new_indices ) , kalman_state . state_len ) ) kalman_state . add_features ( retained_indices , new_indices , state_vec , state_cov , noise_var ) return kalman_state
Return the kalman filter for the features in the new frame

    kalman_state - state from last frame

    old_indices - the index per feature in the last frame or -1 for new

    coordinates - Coordinates of the features in the new frame.

    q - the process error covariance - see equ 1.3 and 1.10 from Welch

    r - measurement error covariance of features - see eqn 1.7 and 1.8 from welch.

    returns a new KalmanState containing the kalman filter of
    the last state by the given coordinates.

    Refer to kalmanGainLinearMotion.m and
    http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

    for info on the algorithm.