public void fill_covariances(MatrixFixed M)
{
//assert (M.Rows() == total_state_size && M.Cols() == total_state_size);
Pxx = M.Extract((int)motion_model.STATE_SIZE, (int)motion_model.STATE_SIZE, 0, 0);
uint x_position = motion_model.STATE_SIZE;
uint x_feature_no = 0;
uint y_feature_no, y_position;
Feature it;
MatrixFixed itmat;
for (int i=0;i<feature_list.Count;i++)
{
if (x_position < M.Columns)
{
it = (Feature)feature_list[i];
y_feature_no = 0;
y_position = 0;
it.set_Pxy(M.Extract((int)motion_model.STATE_SIZE, (int)it.get_feature_measurement_model().FEATURE_STATE_SIZE, (int)y_position, (int)x_position));
y_position += motion_model.STATE_SIZE;
for (int j=0; j < it.matrix_block_list.Count; j++)
{
itmat = (MatrixFixed)(it.matrix_block_list[j]);
//assert (y_feature_no < x_feature_no);
itmat.Update(M.Extract(itmat.Rows, itmat.Columns, (int)y_position, (int)x_position));
y_position += (uint)itmat.Rows;
y_feature_no++;
}
it.set_Pyy(M.Extract((int)it.get_feature_measurement_model().FEATURE_STATE_SIZE,
(int)it.get_feature_measurement_model().FEATURE_STATE_SIZE,
(int)y_position, (int)x_position));
x_feature_no++;
x_position += it.get_feature_measurement_model().FEATURE_STATE_SIZE;
}
}
//assert (x_position <= total_state_size);
}
