Line data    Source code

       1             : #include "AP_Mount_config.h"
       2             : 
       3             : #if HAL_SOLO_GIMBAL_ENABLED
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : 
       7             : // uncomment this to force the optimisation of this code, note that
       8             : // this makes debugging harder
       9             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
      10             : #pragma GCC optimize("O0")
      11             : #else
      12             : #pragma GCC optimize("O2")
      13             : #endif
      14             : 
      15             : #include "SoloGimbalEKF.h"
      16             : #include <AP_Param/AP_Param.h>
      17             : #include <AP_NavEKF/AP_Nav_Common.h>
      18             : #include <AP_AHRS/AP_AHRS.h>
      19             : #include <AP_Compass/AP_Compass.h>
      20             : 
      21             : #include <stdio.h>
      22             : 
      23             : extern const AP_HAL::HAL& hal;
      24             : 
      25             : 
      26             : // Define tuning parameters
      27             : const AP_Param::GroupInfo SoloGimbalEKF::var_info[] = {
      28             :     AP_GROUPEND
      29             : };
      30             : 
      31             : // Hash define constants
      32             : #define GYRO_BIAS_LIMIT 0.349066f // maximum allowed gyro bias (rad/sec)
      33             : 
      34             : // constructor
      35           1 : SoloGimbalEKF::SoloGimbalEKF() :
      36          14 :     states(),
      37           1 :     state(*reinterpret_cast<struct state_elements *>(&states))
      38             : {
      39           1 :     AP_Param::setup_object_defaults(this, var_info);
      40           1 :     reset();
      41           1 : }
      42             : 
      43             : 
      44             : // complete reset
      45           2 : void SoloGimbalEKF::reset()
      46             : {
      47           2 :     ZERO_FARRAY(states);
      48           2 :     memset((void *)&gSense,0,sizeof(gSense));
      49           2 :     memset(&Cov,0,sizeof(Cov));
      50           2 :     TiltCorrectionSquared = 0;
      51           2 :     StartTime_ms = 0;
      52           2 :     FiltInit = false;
      53           2 :     lastMagUpdate = 0;
      54           2 :     dtIMU = 0;
      55           2 :     innovationIncrement = 0;
      56           2 :     lastInnovation = 0;
      57           2 : }
      58             : 
      59             : // run a 9-state EKF used to calculate orientation
      60       18182 : void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
      61             : {
      62       18182 :     imuSampleTime_ms = AP_HAL::millis();
      63       18182 :     dtIMU = delta_time;
      64             : 
      65             :     // initialise variables and constants
      66       18182 :     if (!FiltInit) {
      67             :         // Note: the start time is initialised to 0 in the constructor
      68         463 :         if (StartTime_ms == 0) {
      69           1 :             StartTime_ms = imuSampleTime_ms;
      70             :         }
      71             : 
      72             :         // Set data to pre-initialsation defaults
      73         463 :         FiltInit = false;
      74         463 :         newDataMag = false;
      75         463 :         YawAligned = false;
      76         463 :         memset((void *)&state, 0, sizeof(state));
      77         463 :         state.quat[0] = 1.0f;
      78             : 
      79         463 :         bool main_ekf_healthy = false;
      80             :         nav_filter_status main_ekf_status;
      81             : 
      82         463 :         const auto &_ahrs = AP::ahrs();
      83             : 
      84         463 :         if (_ahrs.get_filter_status(main_ekf_status)) {
      85         463 :             if (main_ekf_status.flags.attitude) {
      86         463 :                 main_ekf_healthy = true;
      87             :             }
      88             :         }
      89             : 
      90             :         // Wait for gimbal to stabilise to body fixed position for a few seconds before starting small EKF
      91             :         // Also wait for navigation EKF to be healthy beasue we are using the velocity output data
      92             :         // This prevents jerky gimbal motion from degrading the EKF initial state estimates
      93         463 :         if (imuSampleTime_ms - StartTime_ms < 5000 || !main_ekf_healthy) {
      94         462 :             return;
      95             :         }
      96             : 
      97           1 :         Quaternion ned_to_vehicle_quat;
      98           1 :         ned_to_vehicle_quat.from_rotation_matrix(_ahrs.get_rotation_body_to_ned());
      99             : 
     100           1 :         Quaternion vehicle_to_gimbal_quat;
     101           1 :         vehicle_to_gimbal_quat.from_vector312(joint_angles.x,joint_angles.y,joint_angles.z);
     102             : 
     103             :         // calculate initial orientation
     104           1 :         state.quat = ned_to_vehicle_quat * vehicle_to_gimbal_quat;
     105             : 
     106           1 :         const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
     107           1 :         const float Sigma_dAngBias = 0.002f*dtIMU; // 1 Sigma uncertainty in delta angle bias (rad)
     108           1 :         const float Sigma_angErr = 0.1f; // 1 Sigma uncertainty in angular misalignment (rad)
     109           4 :         for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
     110           4 :         for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
     111           4 :         for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
     112           1 :         FiltInit = true;
     113           1 :         DEV_PRINTF("\nSoloGimbalEKF Alignment Started\n");
     114             : 
     115             :         // Don't run the filter in this timestep because we have already used the delta velocity data to set an initial orientation
     116           1 :         return;
     117             :     }
     118             : 
     119             :     // We are using IMU data and joint angles from the gimbal
     120       17719 :     gSense.gPsi = joint_angles.z; // yaw
     121       17719 :     gSense.gPhi = joint_angles.x; // roll
     122       17719 :     gSense.gTheta = joint_angles.y; // pitch
     123       17719 :     cosPhi = cosf(gSense.gPhi);
     124       17719 :     cosTheta = cosf(gSense.gTheta);
     125       17719 :     sinPhi = sinf(gSense.gPhi);
     126       17719 :     sinTheta = sinf(gSense.gTheta);
     127       17719 :     sinPsi = sinf(gSense.gPsi);
     128       17719 :     cosPsi = cosf(gSense.gPsi);
     129       17719 :     gSense.delAng = delta_angles;
     130       17719 :     gSense.delVel = delta_velocity;
     131             : 
     132             :     // predict states
     133       17719 :     predictStates();
     134             : 
     135             :     // predict the covariance
     136       17719 :     predictCovariance();
     137             : 
     138             :     // fuse SoloGimbalEKF velocity data
     139       17719 :     fuseVelocity();
     140             : 
     141             :     
     142             :     // Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
     143             :     // Force it to align if too much time has lapsed
     144       17719 :     if (((((imuSampleTime_ms - StartTime_ms) > 8000 && TiltCorrectionSquared < sq(1e-4f)) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
     145             :         //calculate the initial heading using magnetometer, estimated tilt and declination
     146           1 :         alignHeading();
     147           1 :         YawAligned = true;
     148           1 :         DEV_PRINTF("\nSoloGimbalEKF Alignment Completed\n");
     149             :     }
     150             : 
     151             :     // Fuse magnetometer data if  we have new measurements and an aligned heading
     152             :     
     153       17719 :     readMagData();
     154       17719 :     if (newDataMag && YawAligned) {
     155        1889 :         fuseCompass();
     156        1889 :         newDataMag = false;
     157             :     }
     158             :     
     159             : }
     160             : 
     161             : // state prediction
     162       17719 : void SoloGimbalEKF::predictStates()
     163             : {
     164             :     static Vector3f gimDelAngCorrected;
     165             :     static Vector3f gimDelAngPrev;
     166             : 
     167             :     // NED gravity vector m/s^2
     168       17719 :     const Vector3f gravityNED(0, 0, GRAVITY_MSS);
     169             : 
     170             :     // apply corrections for bias and coning errors
     171             :     // % * - and + operators have been overloaded
     172       17719 :     gimDelAngCorrected   = gSense.delAng - state.delAngBias - (gimDelAngPrev % gimDelAngCorrected) * 8.333333e-2f;
     173       17719 :     gimDelAngPrev        = gSense.delAng - state.delAngBias;
     174             : 
     175             :     // update the quaternions by rotating from the previous attitude through
     176             :     // the delta angle rotation quaternion
     177       17719 :     state.quat.rotate(gimDelAngCorrected);
     178             : 
     179             :     // normalise the quaternions and update the quaternion states
     180       17719 :     state.quat.normalize();
     181             : 
     182             :     // calculate the sensor to NED cosine matrix
     183       17719 :     state.quat.rotation_matrix(Tsn);
     184             : 
     185             :     // transform body delta velocities to delta velocities in the nav frame
     186             :     // * and + operators have been overloaded
     187       17719 :     Vector3f delVelNav  = Tsn*gSense.delVel + gravityNED*dtIMU;
     188             : 
     189             :     // sum delta velocities to get velocity
     190       17719 :     state.velocity += delVelNav;
     191             : 
     192       17719 :     state.delAngBias.x = constrain_float(state.delAngBias.x, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
     193       17719 :     state.delAngBias.y = constrain_float(state.delAngBias.y, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
     194       17719 :     state.delAngBias.z = constrain_float(state.delAngBias.z, -GYRO_BIAS_LIMIT*dtIMU,GYRO_BIAS_LIMIT*dtIMU);
     195       17719 : }
     196             : 
     197             : // covariance prediction using optimised algebraic toolbox expressions
     198             : // equivalent to P = F*P*transpose(P) + G*imu_errors*transpose(G) +
     199             : // gyro_bias_state_noise
     200       17719 : void SoloGimbalEKF::predictCovariance()
     201             : {
     202       17719 :     float delAngBiasVariance = sq(dtIMU*5E-6f);
     203       17719 :     if (YawAligned && !hal.util->get_soft_armed()) {
     204        3042 :         delAngBiasVariance *= 4.0f;
     205             :     }
     206             : 
     207       17719 :     float daxNoise = sq(dtIMU*0.0087f);
     208       17719 :     float dayNoise = sq(dtIMU*0.0087f);
     209       17719 :     float dazNoise = sq(dtIMU*0.0087f);
     210             : 
     211       17719 :     float dvxNoise = sq(dtIMU*0.5f);
     212       17719 :     float dvyNoise = sq(dtIMU*0.5f);
     213       17719 :     float dvzNoise = sq(dtIMU*0.5f);
     214       17719 :     float dvx = gSense.delVel.x;
     215       17719 :     float dvy = gSense.delVel.y;
     216       17719 :     float dvz = gSense.delVel.z;
     217       17719 :     float dax = gSense.delAng.x;
     218       17719 :     float day = gSense.delAng.y;
     219       17719 :     float daz = gSense.delAng.z;
     220       17719 :     float q0 = state.quat[0];
     221       17719 :     float q1 = state.quat[1];
     222       17719 :     float q2 = state.quat[2];
     223       17719 :     float q3 = state.quat[3];
     224       17719 :     float dax_b = state.delAngBias.x;
     225       17719 :     float day_b = state.delAngBias.y;
     226       17719 :     float daz_b = state.delAngBias.z;
     227       17719 :     float t1365 = dax*0.5f;
     228       17719 :     float t1366 = dax_b*0.5f;
     229       17719 :     float t1367 = t1365-t1366;
     230       17719 :     float t1368 = day*0.5f;
     231       17719 :     float t1369 = day_b*0.5f;
     232       17719 :     float t1370 = t1368-t1369;
     233       17719 :     float t1371 = daz*0.5f;
     234       17719 :     float t1372 = daz_b*0.5f;
     235       17719 :     float t1373 = t1371-t1372;
     236       17719 :     float t1374 = q2*t1367*0.5f;
     237       17719 :     float t1375 = q1*t1370*0.5f;
     238       17719 :     float t1376 = q0*t1373*0.5f;
     239       17719 :     float t1377 = q2*0.5f;
     240       17719 :     float t1378 = q3*t1367*0.5f;
     241       17719 :     float t1379 = q1*t1373*0.5f;
     242       17719 :     float t1380 = q1*0.5f;
     243       17719 :     float t1381 = q0*t1367*0.5f;
     244       17719 :     float t1382 = q3*t1370*0.5f;
     245       17719 :     float t1383 = q0*0.5f;
     246       17719 :     float t1384 = q2*t1370*0.5f;
     247       17719 :     float t1385 = q3*t1373*0.5f;
     248       17719 :     float t1386 = q0*t1370*0.5f;
     249       17719 :     float t1387 = q3*0.5f;
     250       17719 :     float t1388 = q1*t1367*0.5f;
     251       17719 :     float t1389 = q2*t1373*0.5f;
     252       17719 :     float t1390 = t1374+t1375+t1376-t1387;
     253       17719 :     float t1391 = t1377+t1378+t1379-t1386;
     254       17719 :     float t1392 = q2*t1391*2.0f;
     255       17719 :     float t1393 = t1380+t1381+t1382-t1389;
     256       17719 :     float t1394 = q1*t1393*2.0f;
     257       17719 :     float t1395 = t1383+t1384+t1385-t1388;
     258       17719 :     float t1396 = q0*t1395*2.0f;
     259       17719 :     float t1403 = q3*t1390*2.0f;
     260       17719 :     float t1397 = t1392+t1394+t1396-t1403;
     261       17719 :     float t1398 = sq(q0);
     262       17719 :     float t1399 = sq(q1);
     263       17719 :     float t1400 = sq(q2);
     264       17719 :     float t1401 = sq(q3);
     265       17719 :     float t1402 = t1398+t1399+t1400+t1401;
     266       17719 :     float t1404 = t1374+t1375-t1376+t1387;
     267       17719 :     float t1405 = t1377-t1378+t1379+t1386;
     268       17719 :     float t1406 = q1*t1405*2.0f;
     269       17719 :     float t1407 = -t1380+t1381+t1382+t1389;
     270       17719 :     float t1408 = q2*t1407*2.0f;
     271       17719 :     float t1409 = t1383-t1384+t1385+t1388;
     272       17719 :     float t1410 = q3*t1409*2.0f;
     273       17719 :     float t1420 = q0*t1404*2.0f;
     274       17719 :     float t1411 = t1406+t1408+t1410-t1420;
     275       17719 :     float t1412 = -t1377+t1378+t1379+t1386;
     276       17719 :     float t1413 = q0*t1412*2.0f;
     277       17719 :     float t1414 = t1374-t1375+t1376+t1387;
     278       17719 :     float t1415 = t1383+t1384-t1385+t1388;
     279       17719 :     float t1416 = q2*t1415*2.0f;
     280       17719 :     float t1417 = t1380-t1381+t1382+t1389;
     281       17719 :     float t1418 = q3*t1417*2.0f;
     282       17719 :     float t1421 = q1*t1414*2.0f;
     283       17719 :     float t1419 = t1413+t1416+t1418-t1421;
     284       17719 :     float t1422 = Cov[0][0]*t1397;
     285       17719 :     float t1423 = Cov[1][0]*t1411;
     286       17719 :     float t1429 = Cov[6][0]*t1402;
     287       17719 :     float t1430 = Cov[2][0]*t1419;
     288       17719 :     float t1424 = t1422+t1423-t1429-t1430;
     289       17719 :     float t1425 = Cov[0][1]*t1397;
     290       17719 :     float t1426 = Cov[1][1]*t1411;
     291       17719 :     float t1427 = Cov[0][2]*t1397;
     292       17719 :     float t1428 = Cov[1][2]*t1411;
     293       17719 :     float t1434 = Cov[6][1]*t1402;
     294       17719 :     float t1435 = Cov[2][1]*t1419;
     295       17719 :     float t1431 = t1425+t1426-t1434-t1435;
     296       17719 :     float t1442 = Cov[6][2]*t1402;
     297       17719 :     float t1443 = Cov[2][2]*t1419;
     298       17719 :     float t1432 = t1427+t1428-t1442-t1443;
     299       17719 :     float t1433 = t1398+t1399-t1400-t1401;
     300       17719 :     float t1436 = q0*q2*2.0f;
     301       17719 :     float t1437 = q1*q3*2.0f;
     302       17719 :     float t1438 = t1436+t1437;
     303       17719 :     float t1439 = q0*q3*2.0f;
     304       17719 :     float t1441 = q1*q2*2.0f;
     305       17719 :     float t1440 = t1439-t1441;
     306       17719 :     float t1444 = t1398-t1399+t1400-t1401;
     307       17719 :     float t1445 = q0*q1*2.0f;
     308       17719 :     float t1449 = q2*q3*2.0f;
     309       17719 :     float t1446 = t1445-t1449;
     310       17719 :     float t1447 = t1439+t1441;
     311       17719 :     float t1448 = t1398-t1399-t1400+t1401;
     312       17719 :     float t1450 = t1445+t1449;
     313       17719 :     float t1451 = t1436-t1437;
     314       17719 :     float t1452 = Cov[0][6]*t1397;
     315       17719 :     float t1453 = Cov[1][6]*t1411;
     316       17719 :     float t1628 = Cov[6][6]*t1402;
     317       17719 :     float t1454 = t1452+t1453-t1628-Cov[2][6]*t1419;
     318       17719 :     float t1455 = Cov[0][7]*t1397;
     319       17719 :     float t1456 = Cov[1][7]*t1411;
     320       17719 :     float t1629 = Cov[6][7]*t1402;
     321       17719 :     float t1457 = t1455+t1456-t1629-Cov[2][7]*t1419;
     322       17719 :     float t1458 = Cov[0][8]*t1397;
     323       17719 :     float t1459 = Cov[1][8]*t1411;
     324       17719 :     float t1630 = Cov[6][8]*t1402;
     325       17719 :     float t1460 = t1458+t1459-t1630-Cov[2][8]*t1419;
     326       17719 :     float t1461 = q0*t1390*2.0f;
     327       17719 :     float t1462 = q1*t1391*2.0f;
     328       17719 :     float t1463 = q3*t1395*2.0f;
     329       17719 :     float t1473 = q2*t1393*2.0f;
     330       17719 :     float t1464 = t1461+t1462+t1463-t1473;
     331       17719 :     float t1465 = q0*t1409*2.0f;
     332       17719 :     float t1466 = q2*t1405*2.0f;
     333       17719 :     float t1467 = q3*t1404*2.0f;
     334       17719 :     float t1474 = q1*t1407*2.0f;
     335       17719 :     float t1468 = t1465+t1466+t1467-t1474;
     336       17719 :     float t1469 = q1*t1415*2.0f;
     337       17719 :     float t1470 = q2*t1414*2.0f;
     338       17719 :     float t1471 = q3*t1412*2.0f;
     339       17719 :     float t1475 = q0*t1417*2.0f;
     340       17719 :     float t1472 = t1469+t1470+t1471-t1475;
     341       17719 :     float t1476 = Cov[7][0]*t1402;
     342       17719 :     float t1477 = Cov[0][0]*t1464;
     343       17719 :     float t1486 = Cov[1][0]*t1468;
     344       17719 :     float t1487 = Cov[2][0]*t1472;
     345       17719 :     float t1478 = t1476+t1477-t1486-t1487;
     346       17719 :     float t1479 = Cov[7][1]*t1402;
     347       17719 :     float t1480 = Cov[0][1]*t1464;
     348       17719 :     float t1492 = Cov[1][1]*t1468;
     349       17719 :     float t1493 = Cov[2][1]*t1472;
     350       17719 :     float t1481 = t1479+t1480-t1492-t1493;
     351       17719 :     float t1482 = Cov[7][2]*t1402;
     352       17719 :     float t1483 = Cov[0][2]*t1464;
     353       17719 :     float t1498 = Cov[1][2]*t1468;
     354       17719 :     float t1499 = Cov[2][2]*t1472;
     355       17719 :     float t1484 = t1482+t1483-t1498-t1499;
     356       17719 :     float t1485 = sq(t1402);
     357       17719 :     float t1488 = q1*t1390*2.0f;
     358       17719 :     float t1489 = q2*t1395*2.0f;
     359       17719 :     float t1490 = q3*t1393*2.0f;
     360       17719 :     float t1533 = q0*t1391*2.0f;
     361       17719 :     float t1491 = t1488+t1489+t1490-t1533;
     362       17719 :     float t1494 = q0*t1407*2.0f;
     363       17719 :     float t1495 = q1*t1409*2.0f;
     364       17719 :     float t1496 = q2*t1404*2.0f;
     365       17719 :     float t1534 = q3*t1405*2.0f;
     366       17719 :     float t1497 = t1494+t1495+t1496-t1534;
     367       17719 :     float t1500 = q0*t1415*2.0f;
     368       17719 :     float t1501 = q1*t1417*2.0f;
     369       17719 :     float t1502 = q3*t1414*2.0f;
     370       17719 :     float t1535 = q2*t1412*2.0f;
     371       17719 :     float t1503 = t1500+t1501+t1502-t1535;
     372       17719 :     float t1504 = dvy*t1433;
     373       17719 :     float t1505 = dvx*t1440;
     374       17719 :     float t1506 = t1504+t1505;
     375       17719 :     float t1507 = dvx*t1438;
     376       17719 :     float t1508 = dvy*t1438;
     377       17719 :     float t1509 = dvz*t1440;
     378       17719 :     float t1510 = t1508+t1509;
     379       17719 :     float t1511 = dvx*t1444;
     380       17719 :     float t1551 = dvy*t1447;
     381       17719 :     float t1512 = t1511-t1551;
     382       17719 :     float t1513 = dvz*t1444;
     383       17719 :     float t1514 = dvy*t1446;
     384       17719 :     float t1515 = t1513+t1514;
     385       17719 :     float t1516 = dvx*t1446;
     386       17719 :     float t1517 = dvz*t1447;
     387       17719 :     float t1518 = t1516+t1517;
     388       17719 :     float t1519 = dvx*t1448;
     389       17719 :     float t1520 = dvz*t1451;
     390       17719 :     float t1521 = t1519+t1520;
     391       17719 :     float t1522 = dvy*t1448;
     392       17719 :     float t1552 = dvz*t1450;
     393       17719 :     float t1523 = t1522-t1552;
     394       17719 :     float t1524 = dvx*t1450;
     395       17719 :     float t1525 = dvy*t1451;
     396       17719 :     float t1526 = t1524+t1525;
     397       17719 :     float t1527 = Cov[7][6]*t1402;
     398       17719 :     float t1528 = Cov[0][6]*t1464;
     399       17719 :     float t1529 = Cov[7][7]*t1402;
     400       17719 :     float t1530 = Cov[0][7]*t1464;
     401       17719 :     float t1531 = Cov[7][8]*t1402;
     402       17719 :     float t1532 = Cov[0][8]*t1464;
     403       17719 :     float t1536 = Cov[8][0]*t1402;
     404       17719 :     float t1537 = Cov[1][0]*t1497;
     405       17719 :     float t1545 = Cov[0][0]*t1491;
     406       17719 :     float t1546 = Cov[2][0]*t1503;
     407       17719 :     float t1538 = t1536+t1537-t1545-t1546;
     408       17719 :     float t1539 = Cov[8][1]*t1402;
     409       17719 :     float t1540 = Cov[1][1]*t1497;
     410       17719 :     float t1547 = Cov[0][1]*t1491;
     411       17719 :     float t1548 = Cov[2][1]*t1503;
     412       17719 :     float t1541 = t1539+t1540-t1547-t1548;
     413       17719 :     float t1542 = Cov[8][2]*t1402;
     414       17719 :     float t1543 = Cov[1][2]*t1497;
     415       17719 :     float t1549 = Cov[0][2]*t1491;
     416       17719 :     float t1550 = Cov[2][2]*t1503;
     417       17719 :     float t1544 = t1542+t1543-t1549-t1550;
     418       17719 :     float t1553 = Cov[8][6]*t1402;
     419       17719 :     float t1554 = Cov[1][6]*t1497;
     420       17719 :     float t1555 = Cov[8][7]*t1402;
     421       17719 :     float t1556 = Cov[1][7]*t1497;
     422       17719 :     float t1557 = Cov[8][8]*t1402;
     423       17719 :     float t1558 = Cov[1][8]*t1497;
     424       17719 :     float t1560 = dvz*t1433;
     425       17719 :     float t1559 = t1507-t1560;
     426       17719 :     float t1561 = Cov[0][0]*t1510;
     427       17719 :     float t1567 = Cov[2][0]*t1506;
     428       17719 :     float t1568 = Cov[1][0]*t1559;
     429       17719 :     float t1562 = Cov[3][0]+t1561-t1567-t1568;
     430       17719 :     float t1563 = Cov[0][1]*t1510;
     431       17719 :     float t1569 = Cov[2][1]*t1506;
     432       17719 :     float t1570 = Cov[1][1]*t1559;
     433       17719 :     float t1564 = Cov[3][1]+t1563-t1569-t1570;
     434       17719 :     float t1565 = Cov[0][2]*t1510;
     435       17719 :     float t1571 = Cov[2][2]*t1506;
     436       17719 :     float t1572 = Cov[1][2]*t1559;
     437       17719 :     float t1566 = Cov[3][2]+t1565-t1571-t1572;
     438       17719 :     float t1573 = -t1507+t1560;
     439       17719 :     float t1574 = Cov[1][0]*t1573;
     440       17719 :     float t1575 = Cov[3][0]+t1561-t1567+t1574;
     441       17719 :     float t1576 = Cov[1][1]*t1573;
     442       17719 :     float t1577 = Cov[3][1]+t1563-t1569+t1576;
     443       17719 :     float t1578 = Cov[1][2]*t1573;
     444       17719 :     float t1579 = Cov[3][2]+t1565-t1571+t1578;
     445       17719 :     float t1580 = Cov[0][6]*t1510;
     446       17719 :     float t1581 = Cov[0][7]*t1510;
     447       17719 :     float t1582 = Cov[0][8]*t1510;
     448       17719 :     float t1583 = Cov[1][0]*t1518;
     449       17719 :     float t1584 = Cov[2][0]*t1512;
     450       17719 :     float t1592 = Cov[0][0]*t1515;
     451       17719 :     float t1585 = Cov[4][0]+t1583+t1584-t1592;
     452       17719 :     float t1586 = Cov[1][1]*t1518;
     453       17719 :     float t1587 = Cov[2][1]*t1512;
     454       17719 :     float t1593 = Cov[0][1]*t1515;
     455       17719 :     float t1588 = Cov[4][1]+t1586+t1587-t1593;
     456       17719 :     float t1589 = Cov[1][2]*t1518;
     457       17719 :     float t1590 = Cov[2][2]*t1512;
     458       17719 :     float t1594 = Cov[0][2]*t1515;
     459       17719 :     float t1591 = Cov[4][2]+t1589+t1590-t1594;
     460       17719 :     float t1595 = dvxNoise*t1433*t1447;
     461       17719 :     float t1596 = Cov[1][6]*t1518;
     462       17719 :     float t1597 = Cov[2][6]*t1512;
     463       17719 :     float t1598 = Cov[4][6]+t1596+t1597-Cov[0][6]*t1515;
     464       17719 :     float t1599 = Cov[1][7]*t1518;
     465       17719 :     float t1600 = Cov[2][7]*t1512;
     466       17719 :     float t1601 = Cov[4][7]+t1599+t1600-Cov[0][7]*t1515;
     467       17719 :     float t1602 = Cov[1][8]*t1518;
     468       17719 :     float t1603 = Cov[2][8]*t1512;
     469       17719 :     float t1604 = Cov[4][8]+t1602+t1603-Cov[0][8]*t1515;
     470       17719 :     float t1605 = Cov[2][0]*t1526;
     471       17719 :     float t1606 = Cov[0][0]*t1523;
     472       17719 :     float t1614 = Cov[1][0]*t1521;
     473       17719 :     float t1607 = Cov[5][0]+t1605+t1606-t1614;
     474       17719 :     float t1608 = Cov[2][1]*t1526;
     475       17719 :     float t1609 = Cov[0][1]*t1523;
     476       17719 :     float t1615 = Cov[1][1]*t1521;
     477       17719 :     float t1610 = Cov[5][1]+t1608+t1609-t1615;
     478       17719 :     float t1611 = Cov[2][2]*t1526;
     479       17719 :     float t1612 = Cov[0][2]*t1523;
     480       17719 :     float t1616 = Cov[1][2]*t1521;
     481       17719 :     float t1613 = Cov[5][2]+t1611+t1612-t1616;
     482       17719 :     float t1617 = dvzNoise*t1438*t1448;
     483       17719 :     float t1618 = dvyNoise*t1444*t1450;
     484       17719 :     float t1619 = Cov[2][6]*t1526;
     485       17719 :     float t1620 = Cov[0][6]*t1523;
     486       17719 :     float t1621 = Cov[5][6]+t1619+t1620-Cov[1][6]*t1521;
     487       17719 :     float t1622 = Cov[2][7]*t1526;
     488       17719 :     float t1623 = Cov[0][7]*t1523;
     489       17719 :     float t1624 = Cov[5][7]+t1622+t1623-Cov[1][7]*t1521;
     490       17719 :     float t1625 = Cov[2][8]*t1526;
     491       17719 :     float t1626 = Cov[0][8]*t1523;
     492       17719 :     float t1627 = Cov[5][8]+t1625+t1626-Cov[1][8]*t1521;
     493             :     float nextCov[9][9];
     494       17719 :     nextCov[0][0] = daxNoise*t1485+t1397*t1424+t1411*t1431-t1419*t1432-t1402*t1454;
     495       17719 :     nextCov[1][0] = -t1397*t1478-t1411*t1481+t1419*t1484+t1402*(t1527+t1528-Cov[1][6]*t1468-Cov[2][6]*t1472);
     496       17719 :     nextCov[2][0] = -t1397*t1538-t1411*t1541+t1419*t1544+t1402*(t1553+t1554-Cov[0][6]*t1491-Cov[2][6]*t1503);
     497       17719 :     nextCov[3][0] = -t1402*(Cov[3][6]+t1580-Cov[2][6]*t1506-Cov[1][6]*t1559)+t1397*t1562+t1411*t1564-t1419*t1566;
     498       17719 :     nextCov[4][0] = t1397*t1585+t1411*t1588-t1402*t1598-t1419*t1591;
     499       17719 :     nextCov[5][0] = t1397*t1607+t1411*t1610-t1402*t1621-t1419*t1613;
     500       17719 :     nextCov[6][0] = -t1628+Cov[6][0]*t1397+Cov[6][1]*t1411-Cov[6][2]*t1419;
     501       17719 :     nextCov[7][0] = -t1527+Cov[7][0]*t1397+Cov[7][1]*t1411-Cov[7][2]*t1419;
     502       17719 :     nextCov[8][0] = -t1553+Cov[8][0]*t1397+Cov[8][1]*t1411-Cov[8][2]*t1419;
     503       17719 :     nextCov[0][1] = -t1402*t1457-t1424*t1464+t1431*t1468+t1432*t1472;
     504       17719 :     nextCov[1][1] = dayNoise*t1485+t1464*t1478-t1468*t1481-t1472*t1484+t1402*(t1529+t1530-Cov[1][7]*t1468-Cov[2][7]*t1472);
     505       17719 :     nextCov[2][1] = t1464*t1538-t1468*t1541-t1472*t1544+t1402*(t1555+t1556-Cov[0][7]*t1491-Cov[2][7]*t1503);
     506       17719 :     nextCov[3][1] = -t1402*(Cov[3][7]+t1581-Cov[2][7]*t1506-Cov[1][7]*t1559)-t1464*t1562+t1468*t1564+t1472*t1566;
     507       17719 :     nextCov[4][1] = -t1402*t1601-t1464*t1585+t1468*t1588+t1472*t1591;
     508       17719 :     nextCov[5][1] = -t1402*t1624-t1464*t1607+t1468*t1610+t1472*t1613;
     509       17719 :     nextCov[6][1] = -t1629-Cov[6][0]*t1464+Cov[6][1]*t1468+Cov[6][2]*t1472;
     510       17719 :     nextCov[7][1] = -t1529-Cov[7][0]*t1464+Cov[7][1]*t1468+Cov[7][2]*t1472;
     511       17719 :     nextCov[8][1] = -t1555-Cov[8][0]*t1464+Cov[8][1]*t1468+Cov[8][2]*t1472;
     512       17719 :     nextCov[0][2] = -t1402*t1460-t1431*t1497+t1432*t1503+t1491*(t1422+t1423-t1429-t1430);
     513       17719 :     nextCov[1][2] = -t1478*t1491+t1481*t1497-t1484*t1503+t1402*(t1531+t1532-Cov[1][8]*t1468-Cov[2][8]*t1472);
     514       17719 :     nextCov[2][2] = dazNoise*t1485-t1491*t1538+t1497*t1541-t1503*t1544+t1402*(t1557+t1558-Cov[0][8]*t1491-Cov[2][8]*t1503);
     515       17719 :     nextCov[3][2] = -t1402*(Cov[3][8]+t1582-Cov[2][8]*t1506-Cov[1][8]*t1559)+t1491*t1562-t1497*t1564+t1503*t1566;
     516       17719 :     nextCov[4][2] = -t1402*t1604+t1491*t1585-t1497*t1588+t1503*t1591;
     517       17719 :     nextCov[5][2] = -t1402*t1627+t1491*t1607-t1497*t1610+t1503*t1613;
     518       17719 :     nextCov[6][2] = -t1630+Cov[6][0]*t1491-Cov[6][1]*t1497+Cov[6][2]*t1503;
     519       17719 :     nextCov[7][2] = -t1531+Cov[7][0]*t1491-Cov[7][1]*t1497+Cov[7][2]*t1503;
     520       17719 :     nextCov[8][2] = -t1557+Cov[8][0]*t1491-Cov[8][1]*t1497+Cov[8][2]*t1503;
     521       17719 :     nextCov[0][3] = Cov[0][3]*t1397+Cov[1][3]*t1411-Cov[2][3]*t1419-Cov[6][3]*t1402-t1432*t1506+t1510*(t1422+t1423-t1429-t1430)-t1559*(t1425+t1426-t1434-t1435);
     522       17719 :     nextCov[1][3] = -Cov[0][3]*t1464-Cov[7][3]*t1402+Cov[1][3]*t1468+Cov[2][3]*t1472-t1478*t1510+t1484*t1506+t1481*t1559;
     523       17719 :     nextCov[2][3] = -Cov[8][3]*t1402+Cov[0][3]*t1491-Cov[1][3]*t1497+Cov[2][3]*t1503-t1510*t1538+t1506*t1544+t1541*t1559;
     524       17719 :     nextCov[3][3] = Cov[3][3]+Cov[0][3]*t1510-Cov[2][3]*t1506+Cov[1][3]*t1573-t1506*t1566+t1510*t1575+t1573*t1577+dvxNoise*sq(t1433)+dvyNoise*sq(t1440)+dvzNoise*sq(t1438);
     525       17719 :     nextCov[4][3] = Cov[4][3]+t1595-Cov[0][3]*t1515+Cov[1][3]*t1518+Cov[2][3]*t1512+t1510*t1585-t1506*t1591+t1573*t1588-dvyNoise*t1440*t1444-dvzNoise*t1438*t1446;
     526       17719 :     nextCov[5][3] = Cov[5][3]+t1617+Cov[0][3]*t1523-Cov[1][3]*t1521+Cov[2][3]*t1526+t1510*t1607-t1506*t1613+t1573*t1610-dvxNoise*t1433*t1451-dvyNoise*t1440*t1450;
     527       17719 :     nextCov[6][3] = Cov[6][3]-Cov[6][2]*t1506+Cov[6][0]*t1510+Cov[6][1]*t1573;
     528       17719 :     nextCov[7][3] = Cov[7][3]-Cov[7][2]*t1506+Cov[7][0]*t1510+Cov[7][1]*t1573;
     529       17719 :     nextCov[8][3] = Cov[8][3]-Cov[8][2]*t1506+Cov[8][0]*t1510+Cov[8][1]*t1573;
     530       17719 :     nextCov[0][4] = Cov[0][4]*t1397+Cov[1][4]*t1411-Cov[2][4]*t1419-Cov[6][4]*t1402-t1424*t1515+t1432*t1512+t1518*(t1425+t1426-t1434-t1435);
     531       17719 :     nextCov[1][4] = -Cov[0][4]*t1464-Cov[7][4]*t1402+Cov[1][4]*t1468+Cov[2][4]*t1472+t1478*t1515-t1484*t1512-t1481*t1518;
     532       17719 :     nextCov[2][4] = -Cov[8][4]*t1402+Cov[0][4]*t1491-Cov[1][4]*t1497+Cov[2][4]*t1503+t1515*t1538-t1512*t1544-t1518*t1541;
     533       17719 :     nextCov[3][4] = Cov[3][4]+t1595+Cov[0][4]*t1510-Cov[2][4]*t1506+Cov[1][4]*t1573-t1515*t1575+t1512*t1579+t1518*t1577-dvyNoise*t1440*t1444-dvzNoise*t1438*t1446;
     534       17719 :     nextCov[4][4] = Cov[4][4]-Cov[0][4]*t1515+Cov[1][4]*t1518+Cov[2][4]*t1512-t1515*t1585+t1512*t1591+t1518*t1588+dvxNoise*sq(t1447)+dvyNoise*sq(t1444)+dvzNoise*sq(t1446);
     535       17719 :     nextCov[5][4] = Cov[5][4]+t1618+Cov[0][4]*t1523-Cov[1][4]*t1521+Cov[2][4]*t1526-t1515*t1607+t1512*t1613+t1518*t1610-dvxNoise*t1447*t1451-dvzNoise*t1446*t1448;
     536       17719 :     nextCov[6][4] = Cov[6][4]+Cov[6][2]*t1512-Cov[6][0]*t1515+Cov[6][1]*t1518;
     537       17719 :     nextCov[7][4] = Cov[7][4]+Cov[7][2]*t1512-Cov[7][0]*t1515+Cov[7][1]*t1518;
     538       17719 :     nextCov[8][4] = Cov[8][4]+Cov[8][2]*t1512-Cov[8][0]*t1515+Cov[8][1]*t1518;
     539       17719 :     nextCov[0][5] = Cov[0][5]*t1397+Cov[1][5]*t1411-Cov[2][5]*t1419-Cov[6][5]*t1402+t1424*t1523-t1431*t1521+t1526*(t1427+t1428-t1442-t1443);
     540       17719 :     nextCov[1][5] = -Cov[0][5]*t1464-Cov[7][5]*t1402+Cov[1][5]*t1468+Cov[2][5]*t1472-t1478*t1523+t1481*t1521-t1484*t1526;
     541       17719 :     nextCov[2][5] = -Cov[8][5]*t1402+Cov[0][5]*t1491-Cov[1][5]*t1497+Cov[2][5]*t1503-t1523*t1538+t1521*t1541-t1526*t1544;
     542       17719 :     nextCov[3][5] = Cov[3][5]+t1617+Cov[0][5]*t1510-Cov[2][5]*t1506+Cov[1][5]*t1573-t1521*t1577+t1523*t1575+t1526*t1579-dvxNoise*t1433*t1451-dvyNoise*t1440*t1450;
     543       17719 :     nextCov[4][5] = Cov[4][5]+t1618-Cov[0][5]*t1515+Cov[1][5]*t1518+Cov[2][5]*t1512+t1523*t1585-t1521*t1588+t1526*t1591-dvxNoise*t1447*t1451-dvzNoise*t1446*t1448;
     544       17719 :     nextCov[5][5] = Cov[5][5]+Cov[0][5]*t1523-Cov[1][5]*t1521+Cov[2][5]*t1526+t1523*t1607-t1521*t1610+t1526*t1613+dvxNoise*sq(t1451)+dvyNoise*sq(t1450)+dvzNoise*sq(t1448);
     545       17719 :     nextCov[6][5] = Cov[6][5]-Cov[6][1]*t1521+Cov[6][0]*t1523+Cov[6][2]*t1526;
     546       17719 :     nextCov[7][5] = Cov[7][5]-Cov[7][1]*t1521+Cov[7][0]*t1523+Cov[7][2]*t1526;
     547       17719 :     nextCov[8][5] = Cov[8][5]-Cov[8][1]*t1521+Cov[8][0]*t1523+Cov[8][2]*t1526;
     548       17719 :     nextCov[0][6] = t1454;
     549       17719 :     nextCov[1][6] = -t1527-t1528+Cov[1][6]*t1468+Cov[2][6]*t1472;
     550       17719 :     nextCov[2][6] = -t1553-t1554+Cov[0][6]*t1491+Cov[2][6]*t1503;
     551       17719 :     nextCov[3][6] = Cov[3][6]+t1580-Cov[2][6]*t1506+Cov[1][6]*t1573;
     552       17719 :     nextCov[4][6] = t1598;
     553       17719 :     nextCov[5][6] = t1621;
     554       17719 :     nextCov[6][6] = Cov[6][6];
     555       17719 :     nextCov[7][6] = Cov[7][6];
     556       17719 :     nextCov[8][6] = Cov[8][6];
     557       17719 :     nextCov[0][7] = t1457;
     558       17719 :     nextCov[1][7] = -t1529-t1530+Cov[1][7]*t1468+Cov[2][7]*t1472;
     559       17719 :     nextCov[2][7] = -t1555-t1556+Cov[0][7]*t1491+Cov[2][7]*t1503;
     560       17719 :     nextCov[3][7] = Cov[3][7]+t1581-Cov[2][7]*t1506+Cov[1][7]*t1573;
     561       17719 :     nextCov[4][7] = t1601;
     562       17719 :     nextCov[5][7] = t1624;
     563       17719 :     nextCov[6][7] = Cov[6][7];
     564       17719 :     nextCov[7][7] = Cov[7][7];
     565       17719 :     nextCov[8][7] = Cov[8][7];
     566       17719 :     nextCov[0][8] = t1460;
     567       17719 :     nextCov[1][8] = -t1531-t1532+Cov[1][8]*t1468+Cov[2][8]*t1472;
     568       17719 :     nextCov[2][8] = -t1557-t1558+Cov[0][8]*t1491+Cov[2][8]*t1503;
     569       17719 :     nextCov[3][8] = Cov[3][8]+t1582-Cov[2][8]*t1506+Cov[1][8]*t1573;
     570       17719 :     nextCov[4][8] = t1604;
     571       17719 :     nextCov[5][8] = t1627;
     572       17719 :     nextCov[6][8] = Cov[6][8];
     573       17719 :     nextCov[7][8] = Cov[7][8];
     574       17719 :     nextCov[8][8] = Cov[8][8];
     575             : 
     576             :     // Add the gyro bias state noise
     577       70876 :     for (uint8_t i=6;i<=8;i++) {
     578       53157 :         nextCov[i][i] = nextCov[i][i] + delAngBiasVariance;
     579             :     }
     580             : 
     581             :     // copy predicted variances whilst constraining to be non-negative
     582      177190 :     for (uint8_t index=0; index<=8; index++) {
     583      159471 :         if (nextCov[index][index] < 0.0f) {
     584           0 :             Cov[index][index] = 0.0f;
     585             :         } else {
     586      159471 :             Cov[index][index] = nextCov[index][index];
     587             :         }
     588             :     }
     589             : 
     590             :     // copy elements to covariance matrix whilst enforcing symmetry
     591      159471 :     for (uint8_t rowIndex=1; rowIndex<=8; rowIndex++) {
     592      779636 :         for (uint8_t colIndex=0; colIndex<=rowIndex-1; colIndex++) {
     593      637884 :             Cov[rowIndex][colIndex] = 0.5f*(nextCov[rowIndex][colIndex] + nextCov[colIndex][rowIndex]);
     594      637884 :             Cov[colIndex][rowIndex] = Cov[rowIndex][colIndex];
     595             :         }
     596             :     }
     597             : 
     598       17719 : }
     599             : 
     600             : // Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns
     601       17719 : void SoloGimbalEKF::fuseVelocity()
     602             : {
     603       17719 :     const auto &_ahrs = AP::ahrs();
     604             : 
     605       17719 :     if (!_ahrs.have_inertial_nav()) {
     606           0 :         return;
     607             :     }
     608             : 
     609       17719 :     float R_OBS = 0.25f;
     610             :     float innovation[3];
     611             :     float varInnov[3];
     612       17719 :     Vector3f angErrVec;
     613             :     uint8_t stateIndex;
     614             :     float K[9];
     615             :     // Fuse measurements sequentially
     616       70876 :     for (uint8_t obsIndex=0;obsIndex<=2;obsIndex++) {
     617       53157 :         stateIndex = 3 + obsIndex;
     618             : 
     619             :         // Calculate the velocity measurement innovation using the SoloGimbalEKF estimate as the observation
     620             :         // if heading isn't aligned, use zero velocity (static assumption)
     621       53157 :         if (YawAligned) {
     622       52326 :             Vector3f measVelNED;
     623             :             nav_filter_status main_ekf_status;
     624             : 
     625       52326 :             if (_ahrs.get_filter_status(main_ekf_status)) {
     626       52326 :                 if (main_ekf_status.flags.horiz_vel) {
     627       52326 :                     UNUSED_RESULT(_ahrs.get_velocity_NED(measVelNED));
     628             :                 }
     629             :             }
     630             : 
     631       52326 :             innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
     632             :         } else {
     633         831 :             innovation[obsIndex] = state.velocity[obsIndex];
     634             :         }
     635             : 
     636             :         // Zero the attitude error states - they represent the incremental error so must be zero before corrections are applied
     637       53157 :         state.angErr.zero();
     638             :         // Calculate the innovation variance
     639       53157 :         varInnov[obsIndex] = Cov[stateIndex][stateIndex] + R_OBS;
     640             :         // Calculate the Kalman gain and correct states, taking advantage of direct state observation
     641      531570 :         for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
     642      478413 :             K[rowIndex] = Cov[rowIndex][stateIndex]/varInnov[obsIndex];
     643      478413 :             states[rowIndex] -= K[rowIndex] * innovation[obsIndex];
     644             :         }
     645             : 
     646             :         // Store tilt error estimate for external monitoring
     647       53157 :         angErrVec = angErrVec + state.angErr;
     648             : 
     649             :         // the first 3 states represent the angular error vector where truth = estimate + error. This is is used to correct the estimated quaternion
     650             :         // Bring the quaternion state estimate back to 'truth' by adding the error
     651       53157 :         state.quat.rotate(state.angErr);
     652             : 
     653             :         // re-normalise the quaternion
     654       53157 :         state.quat.normalize();
     655             : 
     656             :         // Update the covariance
     657      531570 :         for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
     658     4784130 :             for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
     659     4305717 :                 Cov[rowIndex][colIndex] = Cov[rowIndex][colIndex] - K[rowIndex]*Cov[stateIndex][colIndex];
     660             :             }
     661             :         }
     662             : 
     663             :         // force symmetry and constrain diagonals to be non-negative
     664       53157 :         fixCovariance();
     665             :     }
     666             : 
     667             :     // calculate tilt component of angle correction
     668       17719 :     TiltCorrectionSquared = sq(angErrVec.x) + sq(angErrVec.y);
     669             : }
     670             : 
     671             : // check for new magnetometer data and update store measurements if available
     672       17719 : void SoloGimbalEKF::readMagData()
     673             : {
     674       17719 :     Compass &compass = AP::compass();
     675             : 
     676       35438 :     if (compass.use_for_yaw() &&
     677       17719 :         compass.last_update_usec() != lastMagUpdate) {
     678             :         // store time of last measurement update
     679        1919 :         lastMagUpdate = compass.last_update_usec();
     680             : 
     681             :         // read compass data and scale to improve numerical conditioning
     682        1919 :         magData = compass.get_field();
     683             : 
     684             :         // let other processes know that new compass data has arrived
     685        1919 :         newDataMag = true;
     686             : 
     687             :     } else {
     688       15800 :         newDataMag = false;
     689             :     }
     690       17719 : }
     691             : 
     692             : // Fuse compass measurements from autopilot
     693        1889 : void SoloGimbalEKF::fuseCompass()
     694             : {
     695        1889 :     float q0 = state.quat[0];
     696        1889 :     float q1 = state.quat[1];
     697        1889 :     float q2 = state.quat[2];
     698        1889 :     float q3 = state.quat[3];
     699             : 
     700        1889 :     float magX = magData.x;
     701        1889 :     float magY = magData.y;
     702        1889 :     float magZ = magData.z;
     703             : 
     704        1889 :     const float R_MAG = 3e-2f;
     705             : 
     706             :     // Calculate observation Jacobian
     707        1889 :     float t5695 = sq(q0);
     708        1889 :     float t5696 = sq(q1);
     709        1889 :     float t5697 = sq(q2);
     710        1889 :     float t5698 = sq(q3);
     711        1889 :     float t5699 = t5695+t5696-t5697-t5698;
     712        1889 :     float t5702 = q0*q2*2.0f;
     713        1889 :     float t5703 = q1*q3*2.0f;
     714        1889 :     float t5704 = t5702+t5703;
     715        1889 :     float t5705 = q0*q3*2.0f;
     716        1889 :     float t5707 = q1*q2*2.0f;
     717        1889 :     float t5706 = t5705-t5707;
     718        1889 :     float t5708 = cosTheta*sinPsi;
     719        1889 :     float t5709 = sinPhi*sinTheta*cosPsi;
     720        1889 :     float t5710 = t5708+t5709;
     721        1889 :     float t5711 = t5705+t5707;
     722        1889 :     float t5712 = sinTheta*sinPsi;
     723        1889 :     float t5730 = cosTheta*sinPhi*cosPsi;
     724        1889 :     float t5713 = t5712-t5730;
     725        1889 :     float t5714 = q0*q1*2.0f;
     726        1889 :     float t5720 = q2*q3*2.0f;
     727        1889 :     float t5715 = t5714-t5720;
     728        1889 :     float t5716 = t5695-t5696+t5697-t5698;
     729        1889 :     float t5717 = sinTheta*cosPsi;
     730        1889 :     float t5718 = cosTheta*sinPhi*sinPsi;
     731        1889 :     float t5719 = t5717+t5718;
     732        1889 :     float t5721 = cosTheta*cosPsi;
     733        1889 :     float t5735 = sinPhi*sinTheta*sinPsi;
     734        1889 :     float t5722 = t5721-t5735;
     735        1889 :     float t5724 = sinPhi*t5706;
     736        1889 :     float t5725 = cosPhi*sinTheta*t5699;
     737        1889 :     float t5726 = cosPhi*cosTheta*t5704;
     738        1889 :     float t5727 = t5724+t5725-t5726;
     739        1889 :     float t5728 = magZ*t5727;
     740        1889 :     float t5729 = t5699*t5710;
     741        1889 :     float t5731 = t5704*t5713;
     742        1889 :     float t5732 = cosPhi*cosPsi*t5706;
     743        1889 :     float t5733 = t5729+t5731-t5732;
     744        1889 :     float t5734 = magY*t5733;
     745        1889 :     float t5736 = t5699*t5722;
     746        1889 :     float t5737 = t5704*t5719;
     747        1889 :     float t5738 = cosPhi*sinPsi*t5706;
     748        1889 :     float t5739 = t5736+t5737+t5738;
     749        1889 :     float t5740 = magX*t5739;
     750        1889 :     float t5741 = -t5728+t5734+t5740;
     751        1889 :     float t5742 = 1.0f/t5741;
     752        1889 :     float t5743 = sinPhi*t5716;
     753        1889 :     float t5744 = cosPhi*cosTheta*t5715;
     754        1889 :     float t5745 = cosPhi*sinTheta*t5711;
     755        1889 :     float t5746 = -t5743+t5744+t5745;
     756        1889 :     float t5747 = magZ*t5746;
     757        1889 :     float t5748 = t5710*t5711;
     758        1889 :     float t5749 = t5713*t5715;
     759        1889 :     float t5750 = cosPhi*cosPsi*t5716;
     760        1889 :     float t5751 = t5748-t5749+t5750;
     761        1889 :     float t5752 = magY*t5751;
     762        1889 :     float t5753 = t5715*t5719;
     763        1889 :     float t5754 = t5711*t5722;
     764        1889 :     float t5755 = cosPhi*sinPsi*t5716;
     765        1889 :     float t5756 = t5753-t5754+t5755;
     766        1889 :     float t5757 = magX*t5756;
     767        1889 :     float t5758 = t5747-t5752+t5757;
     768        1889 :     float t5759 = t5742*t5758;
     769        1889 :     float t5723 = tanf(t5759);
     770        1889 :     float t5760 = sq(t5723);
     771        1889 :     float t5761 = t5760+1.0f;
     772        1889 :     float t5762 = 1.0f/sq(t5741);
     773             :     float H_MAG[3];
     774        1889 :     H_MAG[0] = -t5761*(t5742*(magZ*(sinPhi*t5715+cosPhi*cosTheta*t5716)+magY*(t5713*t5716+cosPhi*cosPsi*t5715)+magX*(t5716*t5719-cosPhi*sinPsi*t5715))-t5758*t5762*(magZ*(sinPhi*t5704+cosPhi*cosTheta*t5706)+magY*(t5706*t5713+cosPhi*cosPsi*t5704)+magX*(t5706*t5719-cosPhi*sinPsi*t5704)));
     775        1889 :     H_MAG[1] =  t5761*(t5742*(magZ*(cosPhi*cosTheta*t5711-cosPhi*sinTheta*t5715)+magY*(t5711*t5713+t5710*t5715)+magX*(t5711*t5719+t5715*t5722))+t5758*t5762*(magZ*(cosPhi*cosTheta*t5699+cosPhi*sinTheta*t5704)+magY*(t5699*t5713-t5704*t5710)+magX*(t5699*t5719-t5704*t5722)));
     776        1889 :     H_MAG[2] =  t5761*(t5742*(-magZ*(sinPhi*t5711+cosPhi*sinTheta*t5716)+magY*(t5710*t5716-cosPhi*cosPsi*t5711)+magX*(t5716*t5722+cosPhi*sinPsi*t5711))-t5758*t5762*(magZ*(sinPhi*t5699-cosPhi*sinTheta*t5706)+magY*(t5706*t5710+cosPhi*t5699*cosPsi)+magX*(t5706*t5722-cosPhi*t5699*sinPsi)));
     777             : 
     778             :     // Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
     779             :     float PH[3];
     780        1889 :     float varInnov = R_MAG;
     781        7556 :     for (uint8_t rowIndex=0;rowIndex<=2;rowIndex++) {
     782        5667 :         PH[rowIndex] = 0.0f;
     783       22668 :         for (uint8_t colIndex=0;colIndex<=2;colIndex++) {
     784       17001 :             PH[rowIndex] += Cov[rowIndex][colIndex]*H_MAG[colIndex];
     785             :         }
     786        5667 :         varInnov += H_MAG[rowIndex]*PH[rowIndex];
     787             :     }
     788             :     float K_MAG[9];
     789        1889 :     float varInnovInv = 1.0f / varInnov;
     790       18890 :     for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
     791       17001 :         K_MAG[rowIndex] = 0.0f;
     792       68004 :         for (uint8_t colIndex=0;colIndex<=2;colIndex++) {
     793       51003 :             K_MAG[rowIndex] += Cov[rowIndex][colIndex]*H_MAG[colIndex];
     794             :         }
     795       17001 :         K_MAG[rowIndex] *= varInnovInv;
     796             :     }
     797             : 
     798             :     // Calculate the innovation
     799        1889 :     float innovation = calcMagHeadingInnov();
     800             : 
     801             :     // limit the innovation so that initial corrections are not too large
     802        1889 :     if (innovation > 0.5f) {
     803           0 :         innovation = 0.5f;
     804        1889 :     } else if (innovation < -0.5f) {
     805           0 :         innovation = -0.5f;
     806             :     }
     807             : 
     808             :     // correct the state vector
     809        1889 :     state.angErr.zero();
     810       18890 :     for (uint8_t i=0;i<=8;i++) {
     811       17001 :         states[i] -= K_MAG[i] * innovation;
     812             :     }
     813             : 
     814             :     // the first 3 states represent the angular error vector where truth = estimate + error. This is is used to correct the estimated quaternion
     815             :     // Bring the quaternion state estimate back to 'truth' by adding the error
     816        1889 :     state.quat.rotate(state.angErr);
     817             : 
     818             :     // re-normalise the quaternion
     819        1889 :     state.quat.normalize();
     820             : 
     821             :     // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero
     822             :     float HP[9];
     823       18890 :     for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
     824       17001 :         HP[colIndex] = 0.0f;
     825       68004 :         for (uint8_t rowIndex=0;rowIndex<=2;rowIndex++) {
     826       51003 :             HP[colIndex] += H_MAG[rowIndex]*Cov[rowIndex][colIndex];
     827             :         }
     828             :     }
     829       18890 :     for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
     830      170010 :         for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
     831      153009 :             Cov[rowIndex][colIndex] -= K_MAG[rowIndex] * HP[colIndex];
     832             :         }
     833             :     }
     834             : 
     835             :     // force symmetry and constrain diagonals to be non-negative
     836        1889 :     fixCovariance();
     837        1889 : }
     838             : 
     839             : // Perform an initial heading alignment using the magnetic field and assumed declination
     840           1 : void SoloGimbalEKF::alignHeading()
     841             : {
     842             :     // calculate the correction rotation vector in NED frame
     843           1 :     Vector3f deltaRotNED = Vector3f(0,0,-calcMagHeadingInnov());
     844             : 
     845             :     // rotate into sensor frame
     846           1 :     Vector3f angleCorrection = Tsn.transposed()*deltaRotNED;
     847             : 
     848             :     // apply the correction to the quaternion state
     849             :     // Bring the quaternion state estimate back to 'truth' by adding the error
     850           1 :     state.quat.rotate(angleCorrection);
     851             : 
     852             :     // re-normalize the quaternion
     853           1 :     state.quat.normalize();
     854           1 : }
     855             : 
     856             : 
     857             : // Calculate magnetic heading innovation
     858        1890 : float SoloGimbalEKF::calcMagHeadingInnov()
     859             : {
     860             :     // Define rotation from magnetometer to sensor using a 312 rotation sequence
     861        1890 :     Matrix3f Tms;
     862        1890 :     Tms[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta;
     863        1890 :     Tms[1][0] = -sinPsi*cosPhi;
     864        1890 :     Tms[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi;
     865        1890 :     Tms[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta;
     866        1890 :     Tms[1][1] = cosPsi*cosPhi;
     867        1890 :     Tms[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi;
     868        1890 :     Tms[0][2] = -sinTheta*cosPhi;
     869        1890 :     Tms[1][2] = sinPhi;
     870        1890 :     Tms[2][2] = cosTheta*cosPhi;
     871             : 
     872        1890 :     const auto &_ahrs = AP::ahrs();
     873             : 
     874             :     // get earth magnetic field estimate from main ekf if available to take advantage of main ekf magnetic field learning
     875        1890 :     Vector3f earth_magfield = Vector3f(0,0,0);
     876        1890 :     _ahrs.get_mag_field_NED(earth_magfield);
     877             : 
     878             :     float declination;
     879        1890 :     if (!earth_magfield.is_zero()) {
     880        1890 :         declination = atan2f(earth_magfield.y,earth_magfield.x);
     881             :     } else {
     882           0 :         declination = AP::compass().get_declination();
     883             :     }
     884             : 
     885        1890 :     Vector3f body_magfield = Vector3f(0,0,0);
     886        1890 :     _ahrs.get_mag_field_correction(body_magfield);
     887             : 
     888             :     // Define rotation from magnetometer to NED axes
     889        1890 :     Matrix3f Tmn = Tsn*Tms;
     890             : 
     891             :     // rotate magnetic field measured at top plate into NED axes afer applying bias values learnt by main EKF
     892        1890 :     Vector3f magMeasNED = Tmn*(magData - body_magfield);
     893             : 
     894             :     // calculate the innovation where the predicted measurement is the angle wrt magnetic north of the horizontal component of the measured field
     895        1890 :     float innovation = atan2f(magMeasNED.y,magMeasNED.x) - declination;
     896             : 
     897             :     // wrap the innovation so it sits on the range from +-pi
     898        1890 :     if (innovation > M_PI) {
     899           0 :         innovation = innovation - 2*M_PI;
     900        1890 :     } else if (innovation < -M_PI) {
     901           0 :         innovation = innovation + 2*M_PI;
     902             :     }
     903             : 
     904             :     // Unwrap so that a large yaw gyro bias offset that causes the heading to wrap does not lead to continual uncontrolled heading drift
     905        1890 :     if (innovation - lastInnovation > M_PI) {
     906             :         // Angle has wrapped in the positive direction to subtract an additional 2*Pi
     907           0 :         innovationIncrement -= 2*M_PI;
     908        1890 :     } else if (innovation -innovationIncrement < -M_PI) {
     909             :         // Angle has wrapped in the negative direction so add an additional 2*Pi
     910           0 :         innovationIncrement += 2*M_PI;
     911             :     }
     912        1890 :     lastInnovation = innovation;
     913             : 
     914        1890 :     return innovation + innovationIncrement;
     915             : }
     916             : 
     917             : // Force symmmetry and non-negative diagonals on state covarinace matrix
     918       55046 : void SoloGimbalEKF::fixCovariance()
     919             : {
     920             :     // force symmetry
     921      495414 :     for (uint8_t rowIndex=1; rowIndex<=8; rowIndex++) {
     922     2422024 :         for (uint8_t colIndex=0; colIndex<=rowIndex-1; colIndex++) {
     923     1981656 :             Cov[rowIndex][colIndex] = 0.5f*(Cov[rowIndex][colIndex] + Cov[colIndex][rowIndex]);
     924     1981656 :             Cov[colIndex][rowIndex] = Cov[rowIndex][colIndex];
     925             :         }
     926             :     }
     927             : 
     928             :     // constrain diagonals to be non-negative
     929      495414 :     for (uint8_t index=1; index<=8; index++) {
     930      440368 :         if (Cov[index][index] < 0.0f) {
     931           0 :             Cov[index][index] = 0.0f;
     932             :         }
     933             :     }
     934       55046 : }
     935             : 
     936             : // get gyro bias data
     937       36365 : void SoloGimbalEKF::getGyroBias(Vector3f &gyroBias) const
     938             : {
     939       36365 :     if (dtIMU < 1.0e-6f) {
     940           2 :         gyroBias.zero();
     941             :     } else {
     942       36363 :         gyroBias = state.delAngBias / dtIMU;
     943             :     }
     944       36365 : }
     945             : 
     946           0 : void SoloGimbalEKF::setGyroBias(const Vector3f &gyroBias)
     947             : {
     948           0 :     if (dtIMU < 1.0e-6f) {
     949           0 :         return;
     950             :     }
     951           0 :     state.delAngBias = gyroBias * dtIMU;
     952             : }
     953             : 
     954             : 
     955             : // get quaternion data
     956       21341 : void SoloGimbalEKF::getQuat(Quaternion &quat) const
     957             : {
     958       21341 :     quat = state.quat;
     959       21341 : }
     960             : 
     961             : // get filter status - true is aligned
     962        5944 : bool SoloGimbalEKF::getStatus() const
     963             : {
     964        5944 :     float run_time = AP_HAL::millis() - StartTime_ms;
     965        5944 :     return  YawAligned && (run_time > 15000);
     966             : }
     967             : #endif // HAL_SOLO_GIMBAL_ENABLED
     968             :