
Here's the formula I want to reproduce in uBLAS: K=P*Mt*(M*P*Mt+R)^-1
Where K, P, M, and R are matrices, and Mt is the transpose of M.
I have written the following function:
void multi_state_kf::compute_gain(const matrix<double>& dev)
{
matrix<double> temp=prod(M,newP);
matrix<double> transM=trans(M);
matrix<double> A=(prod(temp,transM)+dev);
permutation_matrix