soft iron magnetometer calibration

Hi, here's my first contribution to the project...

I couldn't find a way to do a soft iron calibration on the magnetometer measurements, so here is one way to do it.
Applying a diagonal matrix is the same as rescaling... I don't know if I should keep the two possibilities or only apply the matrix.

Tested on my PX4... apparently works fine.