minor fix
add folders
implement
Merge branch 'covariance-intersection-algorithm' into develop