###### tags: `訊號處理`

# ahrs

校正軌跡的方式？

https://www.itread01.com/content/1545324504.html
上面這篇看懂了

ahrs mahony

https://blog.csdn.net/weixin_40378598/article/details/108133032


下圖兩圖為等效 一個是用尤拉角做選轉 一個是用四元數做旋轉


下圖為從四元數那邊擷取過來的圖
注意上圖和下圖的第三個column 會有一些差異 應該是選轉順序不同造成 而最右下角的元素 有兩個平方項的相加會是常數 在有些程式碼還是有計算 沒有當作常數處理

下面是pid控制器的圖


x-imu的部份程式碼

注意下方 一些正負號有差別而已 應該都只是選轉順序的問題


霆寬丟到群組的流程圖

同樣姿態 但是w可能不同的符號
https://blog.endaq.com/quaternions-for-orientation

模擬時 用自己gen的資料無法產生正常的output 但用順先從感測器上量到的ok

yaw旋轉
pitch 旋轉
roll旋轉

# 磁力計部分相關
https://blog.csdn.net/zhiyu_buliang/article/details/89056519

說明磁力計效果不大？
https://blog.csdn.net/qq_39554681/article/details/90931919

知乎
https://zhuanlan.zhihu.com/p/342703398

**磁力計測得的磁場向量在機體坐標系中，該向量還需通過[公式]變換到地理坐標系中，經過一定處理後，再通過[公式]變換回機體坐標系，從而得到理論地磁向量**



上式的右邊有問題 中間不該為0

基本上 就是要校正四元素 讓其旋轉矩陣的反轉換的x軸對準正北
https://blog.csdn.net/nemol1990/article/details/21870197

# 磁力計 橢圓擬合
https://hackmd.io/gDv4BkYbTUeD3NSS2t6tXg

https://www.fierceelectronics.com/components/compensating-for-tilt-hard-iron-and-soft-iron-effects

https://longnight975551865.wordpress.com/2019/11/23/how-to-calibrate-a-magnetometer/

https://www.vectornav.com/resources/magnetometer-errors-calibration

https://www.fierceelectronics.com/components/compensating-for-tilt-hard-iron-and-soft-iron-effects

source code
https://github.com/kriswiner/MPU9250/blob/master/MPU9250_MS5637_AHRS_t3.ino

========================================

霆寬新版的board

然後創建scene = canvas(...時 把這幾行加上去
scene.forward = vec(-1, 0, 0)
scene.up = vec(0, 0, 1)
scene.camera.pos = vec(-1, 0, 0)

不到400元的線性加速規
https://www.st.com/en/mems-and-sensors/iis3dhhc.html?fbclid=IwAR2VYs_QFGFeWjWc5ZANEVZGv9D7p2ZOoeIMpJNjgmKHWDg9Np8CYkpYRrw#overview

1.找出線性加速度

用最新的q算出v 然後將它從加速規量到的值中減掉？

// Estimated direction of gravity and vector perpendicular to magnetic flux
		halfvx = q1 * q3 - q0 * q2;
		halfvy = q0 * q1 + q2 * q3;
		halfvz = q0 * q0 - 0.5f + q3 * q3;

2.把物體座標系加速度轉成大地座標系加速度

q.inverse*a*q<--??

# eagle pcb軟體 免費 給linux

void magcalMPU9250(float * dest1, float * dest2) 
{
    uint16_t ii = 0, sample_count = 0;
    int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
    int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};

    Serial.println("Mag Calibration: Wave device in a figure eight until done!");
    delay(4000);
  
    // shoot for ~fifteen seconds of mag data
    if(Mmode == 0x02) sample_count = 128;  // at 8 Hz ODR, new mag data is available every 125 ms
    if(Mmode == 0x06) sample_count = 1500;  // at 100 Hz ODR, new mag data is available every 10 ms
    for(ii = 0; ii < sample_count; ii++) {
        readMagData(mag_temp);  // Read the mag data   
        for (int jj = 0; jj < 3; jj++) {
            if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
            if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
        }
        if(Mmode == 0x02) delay(135);  // at 8 Hz ODR, new mag data is available every 125 ms
        if(Mmode == 0x06) delay(12);  // at 100 Hz ODR, new mag data is available every 10 ms
    }

//    Serial.println("mag x min/max:"); Serial.println(mag_max[0]); Serial.println(mag_min[0]);
//    Serial.println("mag y min/max:"); Serial.println(mag_max[1]); Serial.println(mag_min[1]);
//    Serial.println("mag z min/max:"); Serial.println(mag_max[2]); Serial.println(mag_min[2]);

    // Get hard iron correction
    mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
    mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
    mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
    
    dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0];  // save mag biases in G for main program
    dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];   
    dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];  
       
    // Get soft iron correction estimate
    mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts
    mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts
    mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts

    float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
    avg_rad /= 3.0;

    dest2[0] = avg_rad/((float)mag_scale[0]);
    dest2[1] = avg_rad/((float)mag_scale[1]);
    dest2[2] = avg_rad/((float)mag_scale[2]);
  
    Serial.println("Mag Calibration done!");
}

Get soft iron correction estimate後面之後是在寫什麼？

// Get magnetometer calibration from AK8963 ROM
  initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer

上面是啥鬼 9250內部rom的參數？