docs: 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