mirror of https://github.com/ArduPilot/ardupilot
920 lines
36 KiB
C++
920 lines
36 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_HAL.h>
|
|
|
|
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
|
|
|
|
// uncomment this to force the optimisation of this code, note that
|
|
// this makes debugging harder
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
|
#pragma GCC optimize("O0")
|
|
#else
|
|
#pragma GCC optimize("O3")
|
|
#endif
|
|
|
|
#include "AP_SmallEKF.h"
|
|
#include <AP_AHRS.h>
|
|
#include <AP_Param.h>
|
|
#include <AP_Vehicle.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
|
// Define tuning parameters
|
|
const AP_Param::GroupInfo SmallEKF::var_info[] PROGMEM = {
|
|
AP_GROUPEND
|
|
};
|
|
|
|
// constructor
|
|
SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) :
|
|
_ahrs(ahrs),
|
|
_main_ekf(ahrs.get_NavEKF_const()),
|
|
state(*reinterpret_cast<struct state_elements *>(&states)),
|
|
FiltInit(false),
|
|
lastMagUpdate(0)
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
}
|
|
|
|
// run a 9-state EKF used to calculate orientation
|
|
void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles)
|
|
{
|
|
imuSampleTime_ms = hal.scheduler->millis();
|
|
dtIMU = delta_time;
|
|
|
|
// initialise variables and constants
|
|
if (!FiltInit) {
|
|
StartTime_ms = imuSampleTime_ms;
|
|
newDataMag = false;
|
|
YawAligned = false;
|
|
state.quat[0] = 1.0f;
|
|
const float Sigma_velNED = 0.5f; // 1 sigma uncertainty in horizontal velocity components
|
|
const float Sigma_dAngBias = 0.01745f*dtIMU; // 1 Sigma uncertainty in delta angle bias
|
|
const float Sigma_angErr = 1.0f; // 1 Sigma uncertainty in angular misalignment (rad)
|
|
for (uint8_t i=0; i <= 2; i++) Cov[i][i] = sq(Sigma_angErr);
|
|
for (uint8_t i=3; i <= 5; i++) Cov[i][i] = sq(Sigma_velNED);
|
|
for (uint8_t i=6; i <= 8; i++) Cov[i][i] = sq(Sigma_dAngBias);
|
|
FiltInit = true;
|
|
hal.console->printf("SmallEKF Alignment Started\n");
|
|
}
|
|
|
|
// We are using the IMU data from the flight vehicle and setting joint angles to zero for the time being
|
|
gSense.gPsi = joint_angles.z; // yaw
|
|
gSense.gPhi = joint_angles.x; // roll
|
|
gSense.gTheta = joint_angles.y; // pitch
|
|
cosPhi = cosf(gSense.gPhi);
|
|
cosTheta = cosf(gSense.gTheta);
|
|
sinPhi = sinf(gSense.gPhi);
|
|
sinTheta = sinf(gSense.gTheta);
|
|
sinPsi = sinf(gSense.gPsi);
|
|
cosPsi = cosf(gSense.gPsi);
|
|
gSense.delAng = delta_angles;
|
|
gSense.delVel = delta_velocity;
|
|
|
|
// predict states
|
|
predictStates();
|
|
|
|
// predict the covariance
|
|
predictCovariance();
|
|
|
|
// fuse SmallEKF velocity data
|
|
fuseVelocity(YawAligned);
|
|
|
|
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
|
|
if ((((imuSampleTime_ms - StartTime_ms) > 5000 && TiltCorrection < 1e-4f) || ((imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
|
|
//calculate the initial heading using magnetometer, estimated tilt and declination
|
|
alignHeading();
|
|
YawAligned = true;
|
|
hal.console->printf("SmallEKF Alignment Completed\n");
|
|
}
|
|
|
|
// Fuse magnetometer data if we have new measurements and an aligned heading
|
|
readMagData();
|
|
if (newDataMag && YawAligned) {
|
|
fuseCompass();
|
|
newDataMag = false;
|
|
}
|
|
|
|
}
|
|
|
|
// state prediction
|
|
void SmallEKF::predictStates()
|
|
{
|
|
static Vector3f gimDelAngCorrected;
|
|
static Vector3f gimDelAngPrev;
|
|
|
|
// NED gravity vector m/s^2
|
|
const Vector3f gravityNED(0, 0, GRAVITY_MSS);
|
|
|
|
// apply corrections for bias and coning errors
|
|
// % * - and + operators have been overloaded
|
|
gimDelAngCorrected = gSense.delAng - state.delAngBias - (gimDelAngPrev % gimDelAngCorrected) * 8.333333e-2f;
|
|
gimDelAngPrev = gSense.delAng - state.delAngBias;
|
|
|
|
// convert the rotation vector to its equivalent quaternion
|
|
float rotationMag = gimDelAngCorrected.length();
|
|
Quaternion deltaQuat;
|
|
if (rotationMag < 1e-6f)
|
|
{
|
|
deltaQuat[0] = 1.0f;
|
|
deltaQuat[1] = 0.0f;
|
|
deltaQuat[2] = 0.0f;
|
|
deltaQuat[3] = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
deltaQuat[0] = cosf(0.5f * rotationMag);
|
|
float rotScaler = (sinf(0.5f * rotationMag)) / rotationMag;
|
|
deltaQuat[1] = gimDelAngCorrected.x * rotScaler;
|
|
deltaQuat[2] = gimDelAngCorrected.y * rotScaler;
|
|
deltaQuat[3] = gimDelAngCorrected.z * rotScaler;
|
|
}
|
|
|
|
// update the quaternions by rotating from the previous attitude through
|
|
// the delta angle rotation quaternion
|
|
state.quat *= deltaQuat;
|
|
|
|
// normalise the quaternions and update the quaternion states
|
|
state.quat.normalize();
|
|
|
|
// calculate the sensor to NED cosine matrix
|
|
state.quat.rotation_matrix(Tsn);
|
|
|
|
// transform body delta velocities to delta velocities in the nav frame
|
|
// * and + operators have been overloaded
|
|
Vector3f delVelNav = Tsn*gSense.delVel + gravityNED*dtIMU;
|
|
|
|
// sum delta velocities to get velocity
|
|
state.velocity += delVelNav;
|
|
}
|
|
|
|
// covariance prediction using optimised algebraic toolbox expressions
|
|
// equivalent to P = F*P*transpose(P) + G*imu_errors*transpose(G) +
|
|
// gyro_bias_state_noise
|
|
void SmallEKF::predictCovariance()
|
|
{
|
|
float delAngBiasVariance = sq(dtIMU*dtIMU*5E-4f);
|
|
|
|
float daxNoise = sq(dtIMU*0.0087f);
|
|
float dayNoise = sq(dtIMU*0.0087f);
|
|
float dazNoise = sq(dtIMU*0.0087f);
|
|
|
|
float dvxNoise = sq(dtIMU*0.5f);
|
|
float dvyNoise = sq(dtIMU*0.5f);
|
|
float dvzNoise = sq(dtIMU*0.5f);
|
|
float dvx = gSense.delVel.x;
|
|
float dvy = gSense.delVel.y;
|
|
float dvz = gSense.delVel.z;
|
|
float dax = gSense.delAng.x;
|
|
float day = gSense.delAng.y;
|
|
float daz = gSense.delAng.z;
|
|
float q0 = state.quat[0];
|
|
float q1 = state.quat[1];
|
|
float q2 = state.quat[2];
|
|
float q3 = state.quat[3];
|
|
float dax_b = state.delAngBias.x;
|
|
float day_b = state.delAngBias.y;
|
|
float daz_b = state.delAngBias.z;
|
|
float t1365 = dax*0.5f;
|
|
float t1366 = dax_b*0.5f;
|
|
float t1367 = t1365-t1366;
|
|
float t1368 = day*0.5f;
|
|
float t1369 = day_b*0.5f;
|
|
float t1370 = t1368-t1369;
|
|
float t1371 = daz*0.5f;
|
|
float t1372 = daz_b*0.5f;
|
|
float t1373 = t1371-t1372;
|
|
float t1374 = q2*t1367*0.5f;
|
|
float t1375 = q1*t1370*0.5f;
|
|
float t1376 = q0*t1373*0.5f;
|
|
float t1377 = q2*0.5f;
|
|
float t1378 = q3*t1367*0.5f;
|
|
float t1379 = q1*t1373*0.5f;
|
|
float t1380 = q1*0.5f;
|
|
float t1381 = q0*t1367*0.5f;
|
|
float t1382 = q3*t1370*0.5f;
|
|
float t1383 = q0*0.5f;
|
|
float t1384 = q2*t1370*0.5f;
|
|
float t1385 = q3*t1373*0.5f;
|
|
float t1386 = q0*t1370*0.5f;
|
|
float t1387 = q3*0.5f;
|
|
float t1388 = q1*t1367*0.5f;
|
|
float t1389 = q2*t1373*0.5f;
|
|
float t1390 = t1374+t1375+t1376-t1387;
|
|
float t1391 = t1377+t1378+t1379-t1386;
|
|
float t1392 = q2*t1391*2.0f;
|
|
float t1393 = t1380+t1381+t1382-t1389;
|
|
float t1394 = q1*t1393*2.0f;
|
|
float t1395 = t1383+t1384+t1385-t1388;
|
|
float t1396 = q0*t1395*2.0f;
|
|
float t1403 = q3*t1390*2.0f;
|
|
float t1397 = t1392+t1394+t1396-t1403;
|
|
float t1398 = sq(q0);
|
|
float t1399 = sq(q1);
|
|
float t1400 = sq(q2);
|
|
float t1401 = sq(q3);
|
|
float t1402 = t1398+t1399+t1400+t1401;
|
|
float t1404 = t1374+t1375-t1376+t1387;
|
|
float t1405 = t1377-t1378+t1379+t1386;
|
|
float t1406 = q1*t1405*2.0f;
|
|
float t1407 = -t1380+t1381+t1382+t1389;
|
|
float t1408 = q2*t1407*2.0f;
|
|
float t1409 = t1383-t1384+t1385+t1388;
|
|
float t1410 = q3*t1409*2.0f;
|
|
float t1420 = q0*t1404*2.0f;
|
|
float t1411 = t1406+t1408+t1410-t1420;
|
|
float t1412 = -t1377+t1378+t1379+t1386;
|
|
float t1413 = q0*t1412*2.0f;
|
|
float t1414 = t1374-t1375+t1376+t1387;
|
|
float t1415 = t1383+t1384-t1385+t1388;
|
|
float t1416 = q2*t1415*2.0f;
|
|
float t1417 = t1380-t1381+t1382+t1389;
|
|
float t1418 = q3*t1417*2.0f;
|
|
float t1421 = q1*t1414*2.0f;
|
|
float t1419 = t1413+t1416+t1418-t1421;
|
|
float t1422 = Cov[0][0]*t1397;
|
|
float t1423 = Cov[1][0]*t1411;
|
|
float t1429 = Cov[6][0]*t1402;
|
|
float t1430 = Cov[2][0]*t1419;
|
|
float t1424 = t1422+t1423-t1429-t1430;
|
|
float t1425 = Cov[0][1]*t1397;
|
|
float t1426 = Cov[1][1]*t1411;
|
|
float t1427 = Cov[0][2]*t1397;
|
|
float t1428 = Cov[1][2]*t1411;
|
|
float t1434 = Cov[6][1]*t1402;
|
|
float t1435 = Cov[2][1]*t1419;
|
|
float t1431 = t1425+t1426-t1434-t1435;
|
|
float t1442 = Cov[6][2]*t1402;
|
|
float t1443 = Cov[2][2]*t1419;
|
|
float t1432 = t1427+t1428-t1442-t1443;
|
|
float t1433 = t1398+t1399-t1400-t1401;
|
|
float t1436 = q0*q2*2.0f;
|
|
float t1437 = q1*q3*2.0f;
|
|
float t1438 = t1436+t1437;
|
|
float t1439 = q0*q3*2.0f;
|
|
float t1441 = q1*q2*2.0f;
|
|
float t1440 = t1439-t1441;
|
|
float t1444 = t1398-t1399+t1400-t1401;
|
|
float t1445 = q0*q1*2.0f;
|
|
float t1449 = q2*q3*2.0f;
|
|
float t1446 = t1445-t1449;
|
|
float t1447 = t1439+t1441;
|
|
float t1448 = t1398-t1399-t1400+t1401;
|
|
float t1450 = t1445+t1449;
|
|
float t1451 = t1436-t1437;
|
|
float t1452 = Cov[0][6]*t1397;
|
|
float t1453 = Cov[1][6]*t1411;
|
|
float t1628 = Cov[6][6]*t1402;
|
|
float t1454 = t1452+t1453-t1628-Cov[2][6]*t1419;
|
|
float t1455 = Cov[0][7]*t1397;
|
|
float t1456 = Cov[1][7]*t1411;
|
|
float t1629 = Cov[6][7]*t1402;
|
|
float t1457 = t1455+t1456-t1629-Cov[2][7]*t1419;
|
|
float t1458 = Cov[0][8]*t1397;
|
|
float t1459 = Cov[1][8]*t1411;
|
|
float t1630 = Cov[6][8]*t1402;
|
|
float t1460 = t1458+t1459-t1630-Cov[2][8]*t1419;
|
|
float t1461 = q0*t1390*2.0f;
|
|
float t1462 = q1*t1391*2.0f;
|
|
float t1463 = q3*t1395*2.0f;
|
|
float t1473 = q2*t1393*2.0f;
|
|
float t1464 = t1461+t1462+t1463-t1473;
|
|
float t1465 = q0*t1409*2.0f;
|
|
float t1466 = q2*t1405*2.0f;
|
|
float t1467 = q3*t1404*2.0f;
|
|
float t1474 = q1*t1407*2.0f;
|
|
float t1468 = t1465+t1466+t1467-t1474;
|
|
float t1469 = q1*t1415*2.0f;
|
|
float t1470 = q2*t1414*2.0f;
|
|
float t1471 = q3*t1412*2.0f;
|
|
float t1475 = q0*t1417*2.0f;
|
|
float t1472 = t1469+t1470+t1471-t1475;
|
|
float t1476 = Cov[7][0]*t1402;
|
|
float t1477 = Cov[0][0]*t1464;
|
|
float t1486 = Cov[1][0]*t1468;
|
|
float t1487 = Cov[2][0]*t1472;
|
|
float t1478 = t1476+t1477-t1486-t1487;
|
|
float t1479 = Cov[7][1]*t1402;
|
|
float t1480 = Cov[0][1]*t1464;
|
|
float t1492 = Cov[1][1]*t1468;
|
|
float t1493 = Cov[2][1]*t1472;
|
|
float t1481 = t1479+t1480-t1492-t1493;
|
|
float t1482 = Cov[7][2]*t1402;
|
|
float t1483 = Cov[0][2]*t1464;
|
|
float t1498 = Cov[1][2]*t1468;
|
|
float t1499 = Cov[2][2]*t1472;
|
|
float t1484 = t1482+t1483-t1498-t1499;
|
|
float t1485 = sq(t1402);
|
|
float t1488 = q1*t1390*2.0f;
|
|
float t1489 = q2*t1395*2.0f;
|
|
float t1490 = q3*t1393*2.0f;
|
|
float t1533 = q0*t1391*2.0f;
|
|
float t1491 = t1488+t1489+t1490-t1533;
|
|
float t1494 = q0*t1407*2.0f;
|
|
float t1495 = q1*t1409*2.0f;
|
|
float t1496 = q2*t1404*2.0f;
|
|
float t1534 = q3*t1405*2.0f;
|
|
float t1497 = t1494+t1495+t1496-t1534;
|
|
float t1500 = q0*t1415*2.0f;
|
|
float t1501 = q1*t1417*2.0f;
|
|
float t1502 = q3*t1414*2.0f;
|
|
float t1535 = q2*t1412*2.0f;
|
|
float t1503 = t1500+t1501+t1502-t1535;
|
|
float t1504 = dvy*t1433;
|
|
float t1505 = dvx*t1440;
|
|
float t1506 = t1504+t1505;
|
|
float t1507 = dvx*t1438;
|
|
float t1508 = dvy*t1438;
|
|
float t1509 = dvz*t1440;
|
|
float t1510 = t1508+t1509;
|
|
float t1511 = dvx*t1444;
|
|
float t1551 = dvy*t1447;
|
|
float t1512 = t1511-t1551;
|
|
float t1513 = dvz*t1444;
|
|
float t1514 = dvy*t1446;
|
|
float t1515 = t1513+t1514;
|
|
float t1516 = dvx*t1446;
|
|
float t1517 = dvz*t1447;
|
|
float t1518 = t1516+t1517;
|
|
float t1519 = dvx*t1448;
|
|
float t1520 = dvz*t1451;
|
|
float t1521 = t1519+t1520;
|
|
float t1522 = dvy*t1448;
|
|
float t1552 = dvz*t1450;
|
|
float t1523 = t1522-t1552;
|
|
float t1524 = dvx*t1450;
|
|
float t1525 = dvy*t1451;
|
|
float t1526 = t1524+t1525;
|
|
float t1527 = Cov[7][6]*t1402;
|
|
float t1528 = Cov[0][6]*t1464;
|
|
float t1529 = Cov[7][7]*t1402;
|
|
float t1530 = Cov[0][7]*t1464;
|
|
float t1531 = Cov[7][8]*t1402;
|
|
float t1532 = Cov[0][8]*t1464;
|
|
float t1536 = Cov[8][0]*t1402;
|
|
float t1537 = Cov[1][0]*t1497;
|
|
float t1545 = Cov[0][0]*t1491;
|
|
float t1546 = Cov[2][0]*t1503;
|
|
float t1538 = t1536+t1537-t1545-t1546;
|
|
float t1539 = Cov[8][1]*t1402;
|
|
float t1540 = Cov[1][1]*t1497;
|
|
float t1547 = Cov[0][1]*t1491;
|
|
float t1548 = Cov[2][1]*t1503;
|
|
float t1541 = t1539+t1540-t1547-t1548;
|
|
float t1542 = Cov[8][2]*t1402;
|
|
float t1543 = Cov[1][2]*t1497;
|
|
float t1549 = Cov[0][2]*t1491;
|
|
float t1550 = Cov[2][2]*t1503;
|
|
float t1544 = t1542+t1543-t1549-t1550;
|
|
float t1553 = Cov[8][6]*t1402;
|
|
float t1554 = Cov[1][6]*t1497;
|
|
float t1555 = Cov[8][7]*t1402;
|
|
float t1556 = Cov[1][7]*t1497;
|
|
float t1557 = Cov[8][8]*t1402;
|
|
float t1558 = Cov[1][8]*t1497;
|
|
float t1560 = dvz*t1433;
|
|
float t1559 = t1507-t1560;
|
|
float t1561 = Cov[0][0]*t1510;
|
|
float t1567 = Cov[2][0]*t1506;
|
|
float t1568 = Cov[1][0]*t1559;
|
|
float t1562 = Cov[3][0]+t1561-t1567-t1568;
|
|
float t1563 = Cov[0][1]*t1510;
|
|
float t1569 = Cov[2][1]*t1506;
|
|
float t1570 = Cov[1][1]*t1559;
|
|
float t1564 = Cov[3][1]+t1563-t1569-t1570;
|
|
float t1565 = Cov[0][2]*t1510;
|
|
float t1571 = Cov[2][2]*t1506;
|
|
float t1572 = Cov[1][2]*t1559;
|
|
float t1566 = Cov[3][2]+t1565-t1571-t1572;
|
|
float t1573 = -t1507+t1560;
|
|
float t1574 = Cov[1][0]*t1573;
|
|
float t1575 = Cov[3][0]+t1561-t1567+t1574;
|
|
float t1576 = Cov[1][1]*t1573;
|
|
float t1577 = Cov[3][1]+t1563-t1569+t1576;
|
|
float t1578 = Cov[1][2]*t1573;
|
|
float t1579 = Cov[3][2]+t1565-t1571+t1578;
|
|
float t1580 = Cov[0][6]*t1510;
|
|
float t1581 = Cov[0][7]*t1510;
|
|
float t1582 = Cov[0][8]*t1510;
|
|
float t1583 = Cov[1][0]*t1518;
|
|
float t1584 = Cov[2][0]*t1512;
|
|
float t1592 = Cov[0][0]*t1515;
|
|
float t1585 = Cov[4][0]+t1583+t1584-t1592;
|
|
float t1586 = Cov[1][1]*t1518;
|
|
float t1587 = Cov[2][1]*t1512;
|
|
float t1593 = Cov[0][1]*t1515;
|
|
float t1588 = Cov[4][1]+t1586+t1587-t1593;
|
|
float t1589 = Cov[1][2]*t1518;
|
|
float t1590 = Cov[2][2]*t1512;
|
|
float t1594 = Cov[0][2]*t1515;
|
|
float t1591 = Cov[4][2]+t1589+t1590-t1594;
|
|
float t1595 = dvxNoise*t1433*t1447;
|
|
float t1596 = Cov[1][6]*t1518;
|
|
float t1597 = Cov[2][6]*t1512;
|
|
float t1598 = Cov[4][6]+t1596+t1597-Cov[0][6]*t1515;
|
|
float t1599 = Cov[1][7]*t1518;
|
|
float t1600 = Cov[2][7]*t1512;
|
|
float t1601 = Cov[4][7]+t1599+t1600-Cov[0][7]*t1515;
|
|
float t1602 = Cov[1][8]*t1518;
|
|
float t1603 = Cov[2][8]*t1512;
|
|
float t1604 = Cov[4][8]+t1602+t1603-Cov[0][8]*t1515;
|
|
float t1605 = Cov[2][0]*t1526;
|
|
float t1606 = Cov[0][0]*t1523;
|
|
float t1614 = Cov[1][0]*t1521;
|
|
float t1607 = Cov[5][0]+t1605+t1606-t1614;
|
|
float t1608 = Cov[2][1]*t1526;
|
|
float t1609 = Cov[0][1]*t1523;
|
|
float t1615 = Cov[1][1]*t1521;
|
|
float t1610 = Cov[5][1]+t1608+t1609-t1615;
|
|
float t1611 = Cov[2][2]*t1526;
|
|
float t1612 = Cov[0][2]*t1523;
|
|
float t1616 = Cov[1][2]*t1521;
|
|
float t1613 = Cov[5][2]+t1611+t1612-t1616;
|
|
float t1617 = dvzNoise*t1438*t1448;
|
|
float t1618 = dvyNoise*t1444*t1450;
|
|
float t1619 = Cov[2][6]*t1526;
|
|
float t1620 = Cov[0][6]*t1523;
|
|
float t1621 = Cov[5][6]+t1619+t1620-Cov[1][6]*t1521;
|
|
float t1622 = Cov[2][7]*t1526;
|
|
float t1623 = Cov[0][7]*t1523;
|
|
float t1624 = Cov[5][7]+t1622+t1623-Cov[1][7]*t1521;
|
|
float t1625 = Cov[2][8]*t1526;
|
|
float t1626 = Cov[0][8]*t1523;
|
|
float t1627 = Cov[5][8]+t1625+t1626-Cov[1][8]*t1521;
|
|
float nextCov[9][9];
|
|
nextCov[0][0] = daxNoise*t1485+t1397*t1424+t1411*t1431-t1419*t1432-t1402*t1454;
|
|
nextCov[1][0] = -t1397*t1478-t1411*t1481+t1419*t1484+t1402*(t1527+t1528-Cov[1][6]*t1468-Cov[2][6]*t1472);
|
|
nextCov[2][0] = -t1397*t1538-t1411*t1541+t1419*t1544+t1402*(t1553+t1554-Cov[0][6]*t1491-Cov[2][6]*t1503);
|
|
nextCov[3][0] = -t1402*(Cov[3][6]+t1580-Cov[2][6]*t1506-Cov[1][6]*t1559)+t1397*t1562+t1411*t1564-t1419*t1566;
|
|
nextCov[4][0] = t1397*t1585+t1411*t1588-t1402*t1598-t1419*t1591;
|
|
nextCov[5][0] = t1397*t1607+t1411*t1610-t1402*t1621-t1419*t1613;
|
|
nextCov[6][0] = -t1628+Cov[6][0]*t1397+Cov[6][1]*t1411-Cov[6][2]*t1419;
|
|
nextCov[7][0] = -t1527+Cov[7][0]*t1397+Cov[7][1]*t1411-Cov[7][2]*t1419;
|
|
nextCov[8][0] = -t1553+Cov[8][0]*t1397+Cov[8][1]*t1411-Cov[8][2]*t1419;
|
|
nextCov[0][1] = -t1402*t1457-t1424*t1464+t1431*t1468+t1432*t1472;
|
|
nextCov[1][1] = dayNoise*t1485+t1464*t1478-t1468*t1481-t1472*t1484+t1402*(t1529+t1530-Cov[1][7]*t1468-Cov[2][7]*t1472);
|
|
nextCov[2][1] = t1464*t1538-t1468*t1541-t1472*t1544+t1402*(t1555+t1556-Cov[0][7]*t1491-Cov[2][7]*t1503);
|
|
nextCov[3][1] = -t1402*(Cov[3][7]+t1581-Cov[2][7]*t1506-Cov[1][7]*t1559)-t1464*t1562+t1468*t1564+t1472*t1566;
|
|
nextCov[4][1] = -t1402*t1601-t1464*t1585+t1468*t1588+t1472*t1591;
|
|
nextCov[5][1] = -t1402*t1624-t1464*t1607+t1468*t1610+t1472*t1613;
|
|
nextCov[6][1] = -t1629-Cov[6][0]*t1464+Cov[6][1]*t1468+Cov[6][2]*t1472;
|
|
nextCov[7][1] = -t1529-Cov[7][0]*t1464+Cov[7][1]*t1468+Cov[7][2]*t1472;
|
|
nextCov[8][1] = -t1555-Cov[8][0]*t1464+Cov[8][1]*t1468+Cov[8][2]*t1472;
|
|
nextCov[0][2] = -t1402*t1460-t1431*t1497+t1432*t1503+t1491*(t1422+t1423-t1429-t1430);
|
|
nextCov[1][2] = -t1478*t1491+t1481*t1497-t1484*t1503+t1402*(t1531+t1532-Cov[1][8]*t1468-Cov[2][8]*t1472);
|
|
nextCov[2][2] = dazNoise*t1485-t1491*t1538+t1497*t1541-t1503*t1544+t1402*(t1557+t1558-Cov[0][8]*t1491-Cov[2][8]*t1503);
|
|
nextCov[3][2] = -t1402*(Cov[3][8]+t1582-Cov[2][8]*t1506-Cov[1][8]*t1559)+t1491*t1562-t1497*t1564+t1503*t1566;
|
|
nextCov[4][2] = -t1402*t1604+t1491*t1585-t1497*t1588+t1503*t1591;
|
|
nextCov[5][2] = -t1402*t1627+t1491*t1607-t1497*t1610+t1503*t1613;
|
|
nextCov[6][2] = -t1630+Cov[6][0]*t1491-Cov[6][1]*t1497+Cov[6][2]*t1503;
|
|
nextCov[7][2] = -t1531+Cov[7][0]*t1491-Cov[7][1]*t1497+Cov[7][2]*t1503;
|
|
nextCov[8][2] = -t1557+Cov[8][0]*t1491-Cov[8][1]*t1497+Cov[8][2]*t1503;
|
|
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);
|
|
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;
|
|
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;
|
|
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);
|
|
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;
|
|
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;
|
|
nextCov[6][3] = Cov[6][3]-Cov[6][2]*t1506+Cov[6][0]*t1510+Cov[6][1]*t1573;
|
|
nextCov[7][3] = Cov[7][3]-Cov[7][2]*t1506+Cov[7][0]*t1510+Cov[7][1]*t1573;
|
|
nextCov[8][3] = Cov[8][3]-Cov[8][2]*t1506+Cov[8][0]*t1510+Cov[8][1]*t1573;
|
|
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);
|
|
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;
|
|
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;
|
|
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;
|
|
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);
|
|
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;
|
|
nextCov[6][4] = Cov[6][4]+Cov[6][2]*t1512-Cov[6][0]*t1515+Cov[6][1]*t1518;
|
|
nextCov[7][4] = Cov[7][4]+Cov[7][2]*t1512-Cov[7][0]*t1515+Cov[7][1]*t1518;
|
|
nextCov[8][4] = Cov[8][4]+Cov[8][2]*t1512-Cov[8][0]*t1515+Cov[8][1]*t1518;
|
|
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);
|
|
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;
|
|
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;
|
|
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;
|
|
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;
|
|
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);
|
|
nextCov[6][5] = Cov[6][5]-Cov[6][1]*t1521+Cov[6][0]*t1523+Cov[6][2]*t1526;
|
|
nextCov[7][5] = Cov[7][5]-Cov[7][1]*t1521+Cov[7][0]*t1523+Cov[7][2]*t1526;
|
|
nextCov[8][5] = Cov[8][5]-Cov[8][1]*t1521+Cov[8][0]*t1523+Cov[8][2]*t1526;
|
|
nextCov[0][6] = t1454;
|
|
nextCov[1][6] = -t1527-t1528+Cov[1][6]*t1468+Cov[2][6]*t1472;
|
|
nextCov[2][6] = -t1553-t1554+Cov[0][6]*t1491+Cov[2][6]*t1503;
|
|
nextCov[3][6] = Cov[3][6]+t1580-Cov[2][6]*t1506+Cov[1][6]*t1573;
|
|
nextCov[4][6] = t1598;
|
|
nextCov[5][6] = t1621;
|
|
nextCov[6][6] = Cov[6][6];
|
|
nextCov[7][6] = Cov[7][6];
|
|
nextCov[8][6] = Cov[8][6];
|
|
nextCov[0][7] = t1457;
|
|
nextCov[1][7] = -t1529-t1530+Cov[1][7]*t1468+Cov[2][7]*t1472;
|
|
nextCov[2][7] = -t1555-t1556+Cov[0][7]*t1491+Cov[2][7]*t1503;
|
|
nextCov[3][7] = Cov[3][7]+t1581-Cov[2][7]*t1506+Cov[1][7]*t1573;
|
|
nextCov[4][7] = t1601;
|
|
nextCov[5][7] = t1624;
|
|
nextCov[6][7] = Cov[6][7];
|
|
nextCov[7][7] = Cov[7][7];
|
|
nextCov[8][7] = Cov[8][7];
|
|
nextCov[0][8] = t1460;
|
|
nextCov[1][8] = -t1531-t1532+Cov[1][8]*t1468+Cov[2][8]*t1472;
|
|
nextCov[2][8] = -t1557-t1558+Cov[0][8]*t1491+Cov[2][8]*t1503;
|
|
nextCov[3][8] = Cov[3][8]+t1582-Cov[2][8]*t1506+Cov[1][8]*t1573;
|
|
nextCov[4][8] = t1604;
|
|
nextCov[5][8] = t1627;
|
|
nextCov[6][8] = Cov[6][8];
|
|
nextCov[7][8] = Cov[7][8];
|
|
nextCov[8][8] = Cov[8][8];
|
|
|
|
// Add the gyro bias state noise
|
|
for (uint8_t i=6;i<=8;i++) {
|
|
nextCov[i][i] = nextCov[i][i] + delAngBiasVariance;
|
|
}
|
|
|
|
// copy predicted variances whilst constraining to be non-negative
|
|
for (uint8_t index=0; index<=8; index++) {
|
|
if (nextCov[index][index] < 0.0f) {
|
|
Cov[index][index] = 0.0f;
|
|
} else {
|
|
Cov[index][index] = nextCov[index][index];
|
|
}
|
|
}
|
|
|
|
// copy elements to covariance matrix whilst enforcing symmetry
|
|
for (uint8_t rowIndex=1; rowIndex<=8; rowIndex++) {
|
|
for (uint8_t colIndex=0; colIndex<=rowIndex-1; colIndex++) {
|
|
Cov[rowIndex][colIndex] = 0.5f*(nextCov[rowIndex][colIndex] + nextCov[colIndex][rowIndex]);
|
|
Cov[colIndex][rowIndex] = Cov[rowIndex][colIndex];
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// Fuse the SmallEKF velocity estimates - this enables alevel reference to be maintained during constant turns
|
|
void SmallEKF::fuseVelocity(bool yawInit)
|
|
{
|
|
float R_OBS = 0.25f;
|
|
float innovation[3];
|
|
float varInnov[3];
|
|
Vector3f angErrVec;
|
|
uint8_t stateIndex;
|
|
float K[9];
|
|
// Fuse measurements sequentially
|
|
for (uint8_t obsIndex=0;obsIndex<=2;obsIndex++) {
|
|
stateIndex = 3 + obsIndex;
|
|
|
|
// Calculate the velocity measurement innovation using the SmallEKF estimate as the observation
|
|
// if heading isn't aligned, use zero velocity (static assumption)
|
|
if (yawInit) {
|
|
Vector3f measVelNED;
|
|
_main_ekf.getVelNED(measVelNED);
|
|
innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
|
|
} else {
|
|
innovation[obsIndex] = state.velocity[obsIndex];
|
|
}
|
|
|
|
// Zero the attitude error states - they represent the incremental error so must be zero before corrections are applied
|
|
state.angErr.zero();
|
|
// Calculate the innovation variance
|
|
varInnov[obsIndex] = Cov[stateIndex][stateIndex] + R_OBS;
|
|
// Calculate the Kalman gain and correct states, taking advantage of direct state observation
|
|
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
|
|
K[rowIndex] = Cov[rowIndex][stateIndex]/varInnov[obsIndex];
|
|
states[rowIndex] -= K[rowIndex] * innovation[obsIndex];
|
|
}
|
|
|
|
// Store tilt error estimate for external monitoring
|
|
angErrVec = angErrVec + state.angErr;
|
|
|
|
// the first 3 states represent the angular misalignment vector. This is
|
|
// is used to correct the estimated quaternion
|
|
// Convert the error rotation vector to its equivalent quaternion
|
|
// truth = estimate + error
|
|
float rotationMag = state.angErr.length();
|
|
if (rotationMag > 1e-6f) {
|
|
Quaternion deltaQuat;
|
|
float temp = sinf(0.5f*rotationMag) / rotationMag;
|
|
deltaQuat[0] = cosf(0.5f*rotationMag);
|
|
deltaQuat[1] = state.angErr.x*temp;
|
|
deltaQuat[2] = state.angErr.y*temp;
|
|
deltaQuat[3] = state.angErr.z*temp;
|
|
|
|
// Update the quaternion states by rotating from the previous attitude through the error quaternion
|
|
state.quat *= deltaQuat;
|
|
|
|
// re-normalise the quaternion
|
|
state.quat.normalize();
|
|
}
|
|
|
|
// Update the covariance
|
|
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
|
|
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
|
|
Cov[rowIndex][colIndex] = Cov[rowIndex][colIndex] - K[rowIndex]*Cov[stateIndex][colIndex];
|
|
}
|
|
}
|
|
|
|
// force symmetry and constrain diagonals to be non-negative
|
|
fixCovariance();
|
|
}
|
|
|
|
// calculate tilt component of angle correction
|
|
TiltCorrection = sqrtf(sq(angErrVec.x) + sq(angErrVec.y));
|
|
}
|
|
|
|
// check for new magnetometer data and update store measurements if available
|
|
void SmallEKF::readMagData()
|
|
{
|
|
if (_ahrs.get_compass() &&
|
|
_ahrs.get_compass()->use_for_yaw() &&
|
|
_ahrs.get_compass()->last_update_usec() != lastMagUpdate) {
|
|
// store time of last measurement update
|
|
lastMagUpdate = _ahrs.get_compass()->last_update_usec();
|
|
|
|
// read compass data and scale to improve numerical conditioning
|
|
magData = _ahrs.get_compass()->get_field() * 0.001f;
|
|
|
|
// let other processes know that new compass data has arrived
|
|
newDataMag = true;
|
|
} else {
|
|
newDataMag = false;
|
|
}
|
|
}
|
|
|
|
// Fuse compass measurements from autopilot
|
|
void SmallEKF::fuseCompass()
|
|
{
|
|
float q0 = state.quat[0];
|
|
float q1 = state.quat[1];
|
|
float q2 = state.quat[2];
|
|
float q3 = state.quat[3];
|
|
|
|
float magX = magData.x;
|
|
float magY = magData.y;
|
|
float magZ = magData.z;
|
|
|
|
const float R_MAG = 3e-2f;
|
|
|
|
// Calculate observation Jacobian
|
|
float t5695 = sq(q0);
|
|
float t5696 = sq(q1);
|
|
float t5697 = sq(q2);
|
|
float t5698 = sq(q3);
|
|
float t5699 = t5695+t5696-t5697-t5698;
|
|
float t5702 = q0*q2*2.0f;
|
|
float t5703 = q1*q3*2.0f;
|
|
float t5704 = t5702+t5703;
|
|
float t5705 = q0*q3*2.0f;
|
|
float t5707 = q1*q2*2.0f;
|
|
float t5706 = t5705-t5707;
|
|
float t5708 = cosTheta*sinPsi;
|
|
float t5709 = sinPhi*sinTheta*cosPsi;
|
|
float t5710 = t5708+t5709;
|
|
float t5711 = t5705+t5707;
|
|
float t5712 = sinTheta*sinPsi;
|
|
float t5730 = cosTheta*sinPhi*cosPsi;
|
|
float t5713 = t5712-t5730;
|
|
float t5714 = q0*q1*2.0f;
|
|
float t5720 = q2*q3*2.0f;
|
|
float t5715 = t5714-t5720;
|
|
float t5716 = t5695-t5696+t5697-t5698;
|
|
float t5717 = sinTheta*cosPsi;
|
|
float t5718 = cosTheta*sinPhi*sinPsi;
|
|
float t5719 = t5717+t5718;
|
|
float t5721 = cosTheta*cosPsi;
|
|
float t5735 = sinPhi*sinTheta*sinPsi;
|
|
float t5722 = t5721-t5735;
|
|
float t5724 = sinPhi*t5706;
|
|
float t5725 = cosPhi*sinTheta*t5699;
|
|
float t5726 = cosPhi*cosTheta*t5704;
|
|
float t5727 = t5724+t5725-t5726;
|
|
float t5728 = magZ*t5727;
|
|
float t5729 = t5699*t5710;
|
|
float t5731 = t5704*t5713;
|
|
float t5732 = cosPhi*cosPsi*t5706;
|
|
float t5733 = t5729+t5731-t5732;
|
|
float t5734 = magY*t5733;
|
|
float t5736 = t5699*t5722;
|
|
float t5737 = t5704*t5719;
|
|
float t5738 = cosPhi*sinPsi*t5706;
|
|
float t5739 = t5736+t5737+t5738;
|
|
float t5740 = magX*t5739;
|
|
float t5741 = -t5728+t5734+t5740;
|
|
float t5742 = 1.0f/t5741;
|
|
float t5743 = sinPhi*t5716;
|
|
float t5744 = cosPhi*cosTheta*t5715;
|
|
float t5745 = cosPhi*sinTheta*t5711;
|
|
float t5746 = -t5743+t5744+t5745;
|
|
float t5747 = magZ*t5746;
|
|
float t5748 = t5710*t5711;
|
|
float t5749 = t5713*t5715;
|
|
float t5750 = cosPhi*cosPsi*t5716;
|
|
float t5751 = t5748-t5749+t5750;
|
|
float t5752 = magY*t5751;
|
|
float t5753 = t5715*t5719;
|
|
float t5754 = t5711*t5722;
|
|
float t5755 = cosPhi*sinPsi*t5716;
|
|
float t5756 = t5753-t5754+t5755;
|
|
float t5757 = magX*t5756;
|
|
float t5758 = t5747-t5752+t5757;
|
|
float t5759 = t5742*t5758;
|
|
float t5723 = tan(t5759);
|
|
float t5760 = sq(t5723);
|
|
float t5761 = t5760+1.0f;
|
|
float t5762 = 1.0f/sq(t5741);
|
|
float H_MAG[3];
|
|
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)));
|
|
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)));
|
|
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)));
|
|
|
|
// Calculate innovation variance and Kalman gains, taking advantage of the fact that only the first 3 elements in H are non zero
|
|
float PH[3];
|
|
float varInnov = R_MAG;
|
|
for (uint8_t rowIndex=0;rowIndex<=2;rowIndex++) {
|
|
PH[rowIndex] = 0.0f;
|
|
for (uint8_t colIndex=0;colIndex<=2;colIndex++) {
|
|
PH[rowIndex] += Cov[rowIndex][colIndex]*H_MAG[colIndex];
|
|
}
|
|
varInnov += H_MAG[rowIndex]*PH[rowIndex];
|
|
}
|
|
float K_MAG[9];
|
|
float varInnovInv = 1.0f / varInnov;
|
|
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
|
|
K_MAG[rowIndex] = 0.0f;
|
|
for (uint8_t colIndex=0;colIndex<=2;colIndex++) {
|
|
K_MAG[rowIndex] += Cov[rowIndex][colIndex]*H_MAG[colIndex];
|
|
}
|
|
K_MAG[rowIndex] *= varInnovInv;
|
|
}
|
|
|
|
// Calculate the innovation
|
|
float innovation = calcMagHeadingInnov();
|
|
|
|
// limit the innovation so that initial corrections are not too large
|
|
if (innovation > 0.5f) {
|
|
innovation = 0.5f;
|
|
} else if (innovation < -0.5f) {
|
|
innovation = -0.5f;
|
|
}
|
|
|
|
// correct the state vector
|
|
state.angErr.zero();
|
|
for (uint8_t i=0;i<=8;i++) {
|
|
states[i] -= K_MAG[i] * innovation;
|
|
}
|
|
|
|
// the first 3 states represent the angular error vector where truth = estimate + error. This is is used to correct the estimated quaternion
|
|
float rotationMag = state.angErr.length();
|
|
if (rotationMag > 1e-6f) {
|
|
// Convert the error rotation vector to its equivalent quaternion
|
|
Quaternion deltaQuat;
|
|
float temp = sinf(0.5f*rotationMag) / rotationMag;
|
|
deltaQuat[0] = cosf(0.5f*rotationMag);
|
|
deltaQuat[1] = state.angErr.x*temp;
|
|
deltaQuat[2] = state.angErr.y*temp;
|
|
deltaQuat[3] = state.angErr.z*temp;
|
|
|
|
// Bring the quaternion state estimate back to 'truth' by adding the error
|
|
state.quat *= deltaQuat;
|
|
|
|
// re-normalise the quaternion
|
|
state.quat.normalize();
|
|
}
|
|
|
|
// 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
|
|
float HP[9];
|
|
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
|
|
HP[colIndex] = 0.0f;
|
|
for (uint8_t rowIndex=0;rowIndex<=2;rowIndex++) {
|
|
HP[colIndex] += H_MAG[rowIndex]*Cov[rowIndex][colIndex];
|
|
}
|
|
}
|
|
for (uint8_t rowIndex=0;rowIndex<=8;rowIndex++) {
|
|
for (uint8_t colIndex=0;colIndex<=8;colIndex++) {
|
|
Cov[rowIndex][colIndex] -= K_MAG[rowIndex] * HP[colIndex];
|
|
}
|
|
}
|
|
|
|
// force symmetry and constrain diagonals to be non-negative
|
|
fixCovariance();
|
|
}
|
|
|
|
// Perform an initial heading alignment using the magnetic field and assumed declination
|
|
void SmallEKF::alignHeading()
|
|
{
|
|
// calculate the correction rotation vector in NED frame
|
|
Vector3f deltaRotNED;
|
|
deltaRotNED.z = -calcMagHeadingInnov();
|
|
|
|
// rotate into sensor frame
|
|
Vector3f angleCorrection = Tsn.transposed()*deltaRotNED;
|
|
|
|
// apply the correction to the quaternion state
|
|
float rotationMag = deltaRotNED.length();
|
|
if (rotationMag > 1e-6f) {
|
|
// Convert the error rotation vector to its equivalent quaternion
|
|
Quaternion deltaQuat;
|
|
float temp = sinf(0.5f*rotationMag) / rotationMag;
|
|
deltaQuat[0] = cosf(0.5f*rotationMag);
|
|
deltaQuat[1] = angleCorrection.x*temp;
|
|
deltaQuat[2] = angleCorrection.y*temp;
|
|
deltaQuat[3] = angleCorrection.z*temp;
|
|
|
|
// Bring the quaternion state estimate back to 'truth' by adding the error
|
|
state.quat *= deltaQuat;
|
|
|
|
// re-normalise the quaternion
|
|
state.quat.normalize();
|
|
}
|
|
}
|
|
|
|
|
|
// Calculate magnetic heading innovation
|
|
float SmallEKF::calcMagHeadingInnov()
|
|
{
|
|
// Define rotation from magnetometer to sensor using a 312 rotation sequence
|
|
Matrix3f Tms;
|
|
Tms[0][0] = cosTheta*cosPsi-sinPsi*sinPhi*sinTheta;
|
|
Tms[1][0] = -sinPsi*cosPhi;
|
|
Tms[2][0] = cosPsi*sinTheta+cosTheta*sinPsi*sinPhi;
|
|
Tms[0][1] = cosTheta*sinPsi+cosPsi*sinPhi*sinTheta;
|
|
Tms[1][1] = cosPsi*cosPhi;
|
|
Tms[2][1] = sinPsi*sinTheta-cosTheta*cosPsi*sinPhi;
|
|
Tms[0][2] = -sinTheta*cosPhi;
|
|
Tms[1][2] = sinPhi;
|
|
Tms[2][2] = cosTheta*cosPhi;
|
|
|
|
// get earth and body magnetic fields
|
|
Vector3f earth_magfield, body_magfield;
|
|
_main_ekf.getMagNED(earth_magfield);
|
|
_main_ekf.getMagXYZ(body_magfield);
|
|
|
|
earth_magfield *= 0.001f;
|
|
body_magfield *= 0.001f;
|
|
|
|
// Define rotation from magnetometer to NED axes
|
|
Matrix3f Tmn = Tsn*Tms;
|
|
// rotate magentic field measured at top plate into NED axes afer applying bias values learnt by SmallEKF
|
|
Vector3f magMeasNED = Tmn*(magData - body_magfield);
|
|
// the predicted measurement is the angle wrt magnetic north of the horizontal component of the measured field
|
|
float innovation = atan2(magMeasNED.y,magMeasNED.x) - atan2(earth_magfield.y,earth_magfield.x);
|
|
|
|
// Unwrap the innovation so it sits on the range from +-pi
|
|
if (innovation > 3.1415927f) {
|
|
innovation = innovation - 6.2831853f;
|
|
} else if (innovation < -3.1415927f) {
|
|
innovation = innovation + 6.2831853f;
|
|
}
|
|
|
|
return innovation;
|
|
}
|
|
|
|
// Force symmmetry and non-negative diagonals on state covarinace matrix
|
|
void SmallEKF::fixCovariance()
|
|
{
|
|
// force symmetry
|
|
for (uint8_t rowIndex=1; rowIndex<=8; rowIndex++) {
|
|
for (uint8_t colIndex=0; colIndex<=rowIndex-1; colIndex++) {
|
|
Cov[rowIndex][colIndex] = 0.5f*(Cov[rowIndex][colIndex] + Cov[colIndex][rowIndex]);
|
|
Cov[colIndex][rowIndex] = Cov[rowIndex][colIndex];
|
|
}
|
|
}
|
|
|
|
// constrain diagonals to be non-negative
|
|
for (uint8_t index=1; index<=8; index++) {
|
|
if (Cov[index][index] < 0.0f) {
|
|
Cov[index][index] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
// return data for debugging EKF
|
|
void SmallEKF::getDebug(float &tilt, Vector3f &velocity, Vector3f &euler, Vector3f &gyroBias) const
|
|
{
|
|
tilt = TiltCorrection;
|
|
velocity = state.velocity;
|
|
state.quat.to_euler(euler.x, euler.y, euler.z);
|
|
if (dtIMU < 1.0e-6) {
|
|
gyroBias.zero();
|
|
} else {
|
|
gyroBias = state.delAngBias / dtIMU;
|
|
}
|
|
}
|
|
|
|
// get gyro bias data
|
|
void SmallEKF::getGyroBias(Vector3f &gyroBias) const
|
|
{
|
|
if (dtIMU < 1.0e-6) {
|
|
gyroBias.zero();
|
|
} else {
|
|
gyroBias = state.delAngBias / dtIMU;
|
|
}
|
|
}
|
|
|
|
// get quaternion data
|
|
void SmallEKF::getQuat(Quaternion &quat) const
|
|
{
|
|
quat = state.quat;
|
|
}
|
|
|
|
#endif // HAL_CPU_CLASS
|