solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: global.cpp
- Revision:
- 141:725321fe2949
- Parent:
- 140:53dbdb207542
- Child:
- 143:53808e4e684c
--- a/global.cpp Mon Dec 06 11:37:55 2021 +0000 +++ b/global.cpp Fri Dec 10 10:43:50 2021 +0000 @@ -96,12 +96,19 @@ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } -//void setDiag(Matrix3f& mat, float val){ -// for (int i = 1; i < mat.getCols()+1; i++){ -// mat(i,i) = val; -// } -//} -// +void setDiag(Matrix3f& mat, float val){ + for (int i = 0; i < mat.cols(); i++){ + mat(i,i) = val; + } +} + +void setDiag(MatrixXf& mat, float val){ + for (int i = 0; i < mat.cols(); i++) + { + mat(i, i) = val; + } +} + //void setBlockDiag(MatrixXf& mat, float val,int startIndex, int endIndex){ // for (int i = startIndex; i < endIndex+1; i++){ // mat(i,i) = val;