UWB distance measurement fusion

We are trying to make use of a UWB system made by Bitcraze, and are in fact also using their Crazyflie 2.1 drone running PX4. Manual flight is no problem, and the drone flies reasonably stable.

Our UWB PX4 driver is based on the same principles as those of the driver written by Bitcraze and the driver for beacon range measurements found in Ardupilot. We are able to use a simple separate three-state EKF to measure the position of the drone using a static position model. This position is fairly stable (within 0.3-meter deviation at most). We are now trying to fuse the measurements of the UWB system with the actual PX4 EKF so that we can also use it with the real dynamic model (i.e. in flight).

Below is the code for fusing the UWB TWR measurements with the PX4 EKF. Before using this function we once determine the static position of the drone and reset the position of the drone to match this. Afterward, the beacon measurements are simply used using the fusion function below.

void Ekf::FuseRNGBeaconData()
{
     // states in the EKF
     uint16_t stateCount = 23;

    // kalman gain values
    Vector24f Kgain;
    Kgain.setZero();

    // the drone's current NED position
    float posNorth = _state.pos(0);
    float posEast = _state.pos(1);
    float posDown = _state.pos(2);

    // beacon position
    float bcnPosNorth = _twr_sample_delayed.beacon_pos_NED(0);
    float bcnPosEast = _twr_sample_delayed.beacon_pos_NED(1);
    float bcnPosDown = _twr_sample_delayed.beacon_pos_NED(2);

    // make sure that the variance of the standard deviation doens't get to optimistically small
    const float R_measurement = sq(fmax(_twr_sample_delayed.std_dev, 0.1f));

	// predicted distance
    Vector3f delta = _state.pos - _twr_sample_delayed.beacon_pos_NED;
    float distPredicted = delta.length();

    // don't use the measurement if we are too close to the beacon for a reliable measurement (either predicted or measured)
    if (distPredicted > 0.5f && _twr_sample_delayed.distance > 0.5f)
    {
        // calculate the innovation
        innovation = distPredicted - _twr_sample_delayed.distance;

        float distPredDown = bcnPosDown-posDown;
        float distPredEast = bcnPosEast-posEast;
        float distPredNorth = bcnPosNorth-posNorth;

	    // the H matrix
	    float H[24];
        memset(H, 0, sizeof(H));
        H[7] = -distPredNorth/distPredicted;
        H[8] = -distPredEast/distPredicted;
        H[9] = -distPredDown/distPredicted;

        // caclulate the total innovation variance
        float tmp2 = H[9] * (P(9,9)*H[9] + P(8,9)*H[8] + P(7,9)*H[7]);
        float tmp1 = H[8] * (P(9,8)*H[9] + P(8,8)*H[8] + P(7,8)*H[7]);
        float tmp0 = H[7] * (P(9,7)*H[9] + P(8,7)*H[8] + P(7,7)*H[7]);

        innovation_var = R_measurement+tmp2+tmp1+tmp0;

        if (innovation_var < R_measurement) {
            // the covariances need to be reset
		    PX4_INFO("reset of the covariance matrix");
            initialiseCovariance();
            return;
        }

	// Quaternions
        Kgain(0) = (P(0,7)* H[7]+P(0,8)*H[8]+P(0,9)*H[9])/innovation_var;
        Kgain(1) = (P(1,7)* H[7]+P(1,8)*H[8]+P(1,9)*H[9])/innovation_var;
        Kgain(2) = (P(2,7)* H[7]+P(2,8)*H[8]+P(2,9)*H[9])/innovation_var;
        Kgain(3) = (P(3,7)* H[7]+P(3,8)*H[8]+P(3,9)*H[9])/innovation_var;

	// Velocity NED (m/s)
        Kgain(4) = (P(4,7)*H[7]+P(4,8)*H[8]+P(4,9)*H[9])/innovation_var;
        Kgain(5) = (P(5,7)*H[7]+P(5,8)*H[8]+P(5,9)*H[9])/innovation_var;
	Kgain(6) = (P(6,7)*H[7]+P(6,8)*H[8]+P(6,9)*H[9])/innovation_var;

	// Position NED (m)
        Kgain(7) = (P(7,7)*H[7]+P(7,8)*H[8]+P(7,9)*H[9])/innovation_var;
        Kgain(8) = (P(8,7)*H[7]+P(8,8)*H[8]+P(8,9)*H[9])/innovation_var;
	Kgain(9) = (P(9,7)*H[7]+P(9,8)*H[8]+P(9,9)*H[9])/innovation_var;

        // IMU delta angle bias XYZ (rad)
        Kgain(10) = (P(10,7)*H[7]+P(10,8)*H[8]+P(10,9)*H[9])/innovation_var;
        Kgain(11) = (P(11,7)*H[7]+P(11,8)*H[8]+P(11,9)*H[9])/innovation_var;
        Kgain(12) = (P(12,7)*H[7]+P(12,8)*H[8]+P(12,9)*H[9])/innovation_var;

        // IMU delta velocity bias XYZ (m/s)
        Kgain(13) = (P(13,7)*H[7]+P(13,8)*H[8]+P(13,9)*H[9])/innovation_var;
        Kgain(14) = (P(14,7)*H[7]+P(14,8)*H[8]+P(14,9)*H[9])/innovation_var;
        Kgain(15) = (P(15,7)*H[7]+P(15,8)*H[8]+P(15,9)*H[9])/innovation_var;

        // Earth magnetic field NED (gauss)
        Kgain(16) = (P(16,7)*H[7]+P(16,8)*H[8]+P(16,9)*H[9])/innovation_var;
        Kgain(17) = (P(17,7)*H[7]+P(17,8)*H[8]+P(17,9)*H[9])/innovation_var;
        Kgain(18) = (P(18,7)*H[7]+P(18,8)*H[8]+P(18,9)*H[9])/innovation_var;

        // Body magnetic field XYZ (gauss)
        Kgain(19) = (P(19,7)*H[7]+P(19,8)*H[8]+P(19,9)*H[9])/innovation_var;
        Kgain(20) = (P(20,7)*H[7]+P(20,8)*H[8]+P(20,9)*H[9])/innovation_var;
        Kgain(21) = (P(21,7)*H[7]+P(21,8)*H[8]+P(21,9)*H[9])/innovation_var;

        // Wind velocity NE (m/s)
        Kgain(22) = (P(22,7)* H[7]+P(22,8)*H[8]+P(22,9)*H[9])/innovation_var;
        Kgain(23) = (P(23,7)* H[7]+P(23,8)*H[8]+P(23,9)*H[9])/innovation_var;


        // update the covariance P = (I - K*H)*P

	rngBcnTestRatio = sq(innovation) / (sq(5) * innovation_var);

		if(rngBcnTestRatio < 1.0f)
		{

			// Calculate KH
			for (uint16_t i = 0; i<=stateCount; i++)
			{
				for (uint16_t j = 0; j <= stateCount; j++)
				{
					KH[i][j] = Kgain(i) * H[j];
				}
			}

			// Calculate KHP
			for (unsigned j = 0; j<=stateCount; j++)
			{
				for (unsigned i = 0; i<=stateCount; i++)
				{
					KHP[i][j] = KH[i][7] * P(7,j) + KH[i][8] * P(8,j) + KH[i][9] * P(9,j);
				}
			}

			// make sure we won't make the covariances negative
			bool healthyFusion = true;
			for (uint8_t i= 0; i<=stateCount; i++)
			{
				if (KHP[i][i] > P(i,i))
				{
					healthyFusion = false;
					PX4_INFO("variances where driven negative in twr fusion");
					P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
				}
			}

			if (healthyFusion) {
				// update the covariance matrix
				for (uint8_t i= 0; i<=stateCount; i++)
				{
					for (uint8_t j= 0; j<=stateCount; j++)
					{
						P(i,j) = P(i,j) - KHP[i][j];
					}
				}

				fixCovarianceErrors(true);

				// update the states
				fuse(Kgain, innovation);

				// needed to let the system know it is not dead reackoning
				_time_last_hor_pos_fuse = _time_last_imu;

			}
		}
		else
		{
			PX4_INFO("twr bad test ratio %f, from beacon: %d, innovation: %f", (double)measurementCount, _twr_sample_delayed.beacon_id, (double)innovation);
		}
    }
}

Using this code and a stationary drone presents me with a correct LOCAL_POSITION_NED in the mavlink inspector. Moving the drone through space by hand also shows a correct tracking of the drone’s position (including a negative value for the height of the drone as it is measured in “downwardness"). Actually flying the drone is impossible, even in manual mode it cannot be controlled/kept in the air. It responds somewhat to the manual control, but has a real mind of its own when using the UWB fusion shown above is active.

Since our drone has limited memory, and because there might be a lot of influences on the received UWB signals, we decided to simulate our fusion software. This SITL simulation uses perfect UWB distance messages, and indeed our separate static position 3-state EKF shows a very stable position (no more than a few centimeters of deviation while the drone is still on the ground). When we try to fly the drone by commanding it to just hover above the ground through liftoff in Q-ground control the drone does lift off, but then quickly gains horizontal speed too and crashes into the ground a few seconds later. Doing the same with only GPS instead of our UWB fusion causes no problems, so it does seem to be purely our UWB fusion and not the simulation setup.

The log file of a simulated flight relying on the UWB fusion can be found here.
A video of this same flight can be found here.

Problems & assumptions I can think of:

No magnetometer present: will the EKF infer the attitude from movements and the resulting NED positions?

The EKF normally requires a magnetometer even to initialise, even when SYS_HAS_MAG is set to 0. I removed this check in the initialisation of the EKF, which does allow me to fly the Crazyflie manually without a magnetometer. This will lead to yaw estimations drifting away of course, but my assumption would be that the EKF would infer the correct yaw from positions/movements measured by the UWB system.

When using the UWB fake measurements there is some drift of the attitude, maybe a few degrees per second but not just in one direction and not at a constant speed. It then still responds to moving it by hand (yaw, and horizon). The drift in yaw a lot more when using real UWB measurements, but not during every test. The NED position itself is also less stable when using those real measurements, so I assume that the yaw is simply influenced by this less stable position.

Am I right in thinking that flying trough the space will make the yaw align with he “North” direction (essentially just the y direction) of the UWB system? Or will the lack of a magnetometer to stabilise the yaw make it impossible to fly?

Erroneous fusion of states

I can for the life of me not find any faults in the fusion algorithm, but I might just be missing something simple. It would be an amazing help if somebody with a bit more knowledge of what the covariances should look like could check the code and log file.