Bug with multiple locations for PCRBMetric, feature for multiple measurement models?

[lwlarsson]

When creating a PCRBMetric with multiple sensor_locations, the PCRBMetric fails to correctly extract the states from the state_vector.

The error is due to

sensor_location = sensor_locations[:, i]

in _calculate_j_z

It is solved by changing it to

sensor_location = StateVector(sensor_locations[:, location_i])

I've never contributed on GitHub before, so I'm unsure how to post this correctly. I guess I'll do it here, please tell me if it is wrong. To extend PCRBMetric to multiple sensor locations with multiple measurement models (at each location), the following changes could be made (information is additive)

    def _calculate_j_z(state, sensor_locations, measurement_models, irf):

        # allocate memory / initialisation
        overall_j_z = np.zeros((state.ndim, state.ndim))

        num_sensor_locations = sensor_locations.shape[1]
        num_sensors = len(measurement_models)

        # precalculate inverses outside of loop
        measurement_cov_invs = []
        for sensor_i in range(num_sensors):
            # inverse of measurement covariance
            measurement_cov_invs.append(inv(measurement_models[sensor_i].covar()))

        
        for location_i in range(num_sensor_locations):
            for sensor_i in range(num_sensors):
                sensor_location = StateVector(sensor_locations[:, location_i])
                measurement_model_cp = copy.copy(measurement_models[sensor_i])
                if hasattr(measurement_model_cp, 'translation_offset'):
                    measurement_model_cp.translation_offset = sensor_location

                h_matrix = measurement_model_cp.jacobian(state)

                j_z = h_matrix.T @ measurement_cov_invs[sensor_i] @ h_matrix

                # increment
                overall_j_z += irf[location_i] * j_z

        return overall_j_z

and

    measurement_models: Sequence[MeasurementModel] = Property(
        doc="The measurement models that projects a track into measurement space "
        "(and vice versa), all sensor models assumed located at each `sensor_location`")

The purpose would be to easily study systems of e.g. bearing-only and radar at multiple locations. Before (assuming the bug is fixed) it would only be possible to study systems consisting of multiple radars or bearing-only sensors.

[lwlarsson]

I guess that the tests are insufficient also, since this case wasn't caught/checked.