#include "eskf.h"

#include <float.h>
#include <math.h>
#include <stddef.h>
#include <string.h>

#define ESKF_POSITION_INDEX 0u
#define ESKF_VELOCITY_INDEX 3u
#define ESKF_ATTITUDE_INDEX 6u
#define ESKF_ACCEL_BIAS_INDEX 9u
#define ESKF_GYRO_BIAS_INDEX 12u

#define ESKF_PI 3.14159265358979323846264338327950288

static double square(double value)
{
	return value * value;
}

static double vector_norm3(const double value[3])
{
	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
}

static bool vector_is_finite(const double* value, size_t count)
{
	if(value == NULL)
	{
		return false;
	}
	for(size_t i = 0 ; i < count ; i++)
	{
		if(!isfinite(value[i]))
		{
			return false;
		}
	}
	return true;
}

static bool quaternion_normalize(double quaternion[4])
{
	double norm = sqrt(square(quaternion[0]) + square(quaternion[1]) +
	                   square(quaternion[2]) + square(quaternion[3]));
	if(!(norm > DBL_EPSILON) || !isfinite(norm))
	{
		return false;
	}
	for(unsigned int i = 0 ; i < 4u ; i++)
	{
		quaternion[i] /= norm;
	}
	return true;
}

static void quaternion_multiply(const double first[4], const double second[4],
	                            double output[4])
{
	output[0] = first[0] * second[0] - first[1] * second[1] -
	            first[2] * second[2] - first[3] * second[3];
	output[1] = first[0] * second[1] + first[1] * second[0] +
	            first[2] * second[3] - first[3] * second[2];
	output[2] = first[0] * second[2] - first[1] * second[3] +
	            first[2] * second[0] + first[3] * second[1];
	output[3] = first[0] * second[3] + first[1] * second[2] -
	            first[2] * second[1] + first[3] * second[0];
}

static void rotation_vector_quaternion(const double rotation_vector[3],
	                                   double quaternion[4])
{
	double angle = vector_norm3(rotation_vector);
	if(angle < 1.0e-10)
	{
		quaternion[0] = 1.0;
		quaternion[1] = 0.5 * rotation_vector[0];
		quaternion[2] = 0.5 * rotation_vector[1];
		quaternion[3] = 0.5 * rotation_vector[2];
		(void)quaternion_normalize(quaternion);
		return;
	}

	double scale = sin(0.5 * angle) / angle;
	quaternion[0] = cos(0.5 * angle);
	quaternion[1] = scale * rotation_vector[0];
	quaternion[2] = scale * rotation_vector[1];
	quaternion[3] = scale * rotation_vector[2];
}

static void quaternion_rotation_matrix(const double quaternion[4],
	                                   double rotation[3][3])
{
	const double w = quaternion[0];
	const double x = quaternion[1];
	const double y = quaternion[2];
	const double z = quaternion[3];

	rotation[0][0] = 1.0 - 2.0 * (y * y + z * z);
	rotation[0][1] = 2.0 * (x * y - w * z);
	rotation[0][2] = 2.0 * (x * z + w * y);
	rotation[1][0] = 2.0 * (x * y + w * z);
	rotation[1][1] = 1.0 - 2.0 * (x * x + z * z);
	rotation[1][2] = 2.0 * (y * z - w * x);
	rotation[2][0] = 2.0 * (x * z - w * y);
	rotation[2][1] = 2.0 * (y * z + w * x);
	rotation[2][2] = 1.0 - 2.0 * (x * x + y * y);
}

static void skew_matrix(const double vector[3], double skew[3][3])
{
	skew[0][0] = 0.0;
	skew[0][1] = -vector[2];
	skew[0][2] = vector[1];
	skew[1][0] = vector[2];
	skew[1][1] = 0.0;
	skew[1][2] = -vector[0];
	skew[2][0] = -vector[1];
	skew[2][1] = vector[0];
	skew[2][2] = 0.0;
}

static bool inverse_symmetric3(const double input[9], double inverse[9])
{
	double lower[3][3] = {{0.0}};
	double scale = fmax(fabs(input[0]), fmax(fabs(input[4]), fabs(input[8])));
	if(!(scale > 0.0) || !isfinite(scale))
	{
		return false;
	}

	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col <= row ; col++)
		{
			double value = 0.5 * (input[row * 3u + col] + input[col * 3u + row]);
			for(unsigned int k = 0 ; k < col ; k++)
			{
				value -= lower[row][k] * lower[col][k];
			}
			if(row == col)
			{
				if(value <= scale * DBL_EPSILON * 128.0 || !isfinite(value))
				{
					return false;
				}
				lower[row][col] = sqrt(value);
			}
			else
			{
				lower[row][col] = value / lower[col][col];
			}
		}
	}

	for(unsigned int column = 0 ; column < 3u ; column++)
	{
		double forward[3] = {0.0, 0.0, 0.0};
		double solution[3] = {0.0, 0.0, 0.0};
		for(unsigned int row = 0 ; row < 3u ; row++)
		{
			double rhs = (row == column) ? 1.0 : 0.0;
			for(unsigned int k = 0 ; k < row ; k++)
			{
				rhs -= lower[row][k] * forward[k];
			}
			forward[row] = rhs / lower[row][row];
		}
		for(int row = 2 ; row >= 0 ; row--)
		{
			double rhs = forward[row];
			for(unsigned int k = (unsigned int)row + 1u ; k < 3u ; k++)
			{
				rhs -= lower[k][row] * solution[k];
			}
			solution[row] = rhs / lower[row][row];
		}
		for(unsigned int row = 0 ; row < 3u ; row++)
		{
			inverse[row * 3u + column] = solution[row];
		}
	}
	return vector_is_finite(inverse, 9u);
}

static void covariance_symmetrize_and_floor(eskf_t* filter, double floor_value)
{
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = row + 1u ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			double symmetric = 0.5 *
				(filter->covariance[row][col] + filter->covariance[col][row]);
			filter->covariance[row][col] = symmetric;
			filter->covariance[col][row] = symmetric;
		}
		if(filter->covariance[row][row] < floor_value)
		{
			filter->covariance[row][row] = floor_value;
		}
	}
}

void eskf_default_config(eskf_config_t* config)
{
	if(config == NULL)
	{
		return;
	}
	memset(config, 0, sizeof(*config));
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		config->accel_noise_density[i] = 0.03;
		config->gyro_noise_density[i] = 0.003;
		config->accel_bias_random_walk[i] = 0.002;
		config->gyro_bias_random_walk[i] = 0.002 * ESKF_PI / 180.0;
		config->initial_position_std_m[i] = (i < 2u) ? 10.0 : 20.0;
		config->initial_velocity_std_mps[i] = 1.0;
		config->initial_attitude_std_rad[i] =
			((i < 2u) ? 5.0 : 30.0) * ESKF_PI / 180.0;
		config->initial_accel_bias_std_mps2[i] = 0.1;
		config->initial_gyro_bias_std_rps[i] = 0.05 * ESKF_PI / 180.0;
	}
	config->gravity_mps2 = 9.80665;
	config->max_propagation_step_s = 0.02;
	config->position_nis_gate = 16.26623619623813; /* chi-square 3 DOF, 99.9%. */
	config->covariance_diagonal_floor = 1.0e-12;
}

bool eskf_quaternion_align_vectors(const double source[3],
	                               const double target[3],
	                               double quaternion_out[4])
{
	if(!vector_is_finite(source, 3u) || !vector_is_finite(target, 3u) ||
	   (quaternion_out == NULL))
	{
		return false;
	}
	double source_norm = vector_norm3(source);
	double target_norm = vector_norm3(target);
	if(!(source_norm > DBL_EPSILON) || !(target_norm > DBL_EPSILON))
	{
		return false;
	}
	double first[3];
	double second[3];
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		first[i] = source[i] / source_norm;
		second[i] = target[i] / target_norm;
	}
	double dot = first[0] * second[0] + first[1] * second[1] + first[2] * second[2];
	if(dot < -0.999999)
	{
		double axis[3] = {0.0, first[2], -first[1]};
		if(vector_norm3(axis) < 1.0e-6)
		{
			axis[0] = -first[2];
			axis[1] = 0.0;
			axis[2] = first[0];
		}
		double axis_norm = vector_norm3(axis);
		quaternion_out[0] = 0.0;
		for(unsigned int i = 0 ; i < 3u ; i++)
		{
			quaternion_out[i + 1u] = axis[i] / axis_norm;
		}
		return true;
	}

	quaternion_out[0] = 1.0 + dot;
	quaternion_out[1] = first[1] * second[2] - first[2] * second[1];
	quaternion_out[2] = first[2] * second[0] - first[0] * second[2];
	quaternion_out[3] = first[0] * second[1] - first[1] * second[0];
	return quaternion_normalize(quaternion_out);
}

bool eskf_initialize(eskf_t* filter,
	                 const eskf_config_t* config,
	                 const double initial_position_m[3],
	                 const double body_to_nav_quaternion[4],
	                 const double accel_bias_mps2[3],
	                 const double gyro_bias_rps[3])
{
	if((filter == NULL) || (config == NULL) ||
	   !vector_is_finite(initial_position_m, 3u) ||
	   !vector_is_finite(body_to_nav_quaternion, 4u) ||
	   !vector_is_finite(accel_bias_mps2, 3u) ||
	   !vector_is_finite(gyro_bias_rps, 3u) ||
	   !(config->gravity_mps2 > 0.0))
	{
		return false;
	}

	memset(filter, 0, sizeof(*filter));
	memcpy(filter->position_m, initial_position_m, sizeof(filter->position_m));
	memcpy(filter->body_to_nav_quaternion, body_to_nav_quaternion,
	       sizeof(filter->body_to_nav_quaternion));
	memcpy(filter->accel_bias_mps2, accel_bias_mps2,
	       sizeof(filter->accel_bias_mps2));
	memcpy(filter->gyro_bias_rps, gyro_bias_rps, sizeof(filter->gyro_bias_rps));
	if(!quaternion_normalize(filter->body_to_nav_quaternion))
	{
		return false;
	}

	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		filter->covariance[ESKF_POSITION_INDEX + i][ESKF_POSITION_INDEX + i] =
			square(config->initial_position_std_m[i]);
		filter->covariance[ESKF_VELOCITY_INDEX + i][ESKF_VELOCITY_INDEX + i] =
			square(config->initial_velocity_std_mps[i]);
		filter->covariance[ESKF_ATTITUDE_INDEX + i][ESKF_ATTITUDE_INDEX + i] =
			square(config->initial_attitude_std_rad[i]);
		filter->covariance[ESKF_ACCEL_BIAS_INDEX + i][ESKF_ACCEL_BIAS_INDEX + i] =
			square(config->initial_accel_bias_std_mps2[i]);
		filter->covariance[ESKF_GYRO_BIAS_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] =
			square(config->initial_gyro_bias_std_rps[i]);
	}
	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
	filter->last_position_nis = NAN;
	filter->initialized = true;
	return true;
}

static bool eskf_propagate_step(eskf_t* filter,
	                            const eskf_config_t* config,
	                            const double accel_mps2[3],
	                            const double gyro_rps[3],
	                            double dt_s)
{
	double specific_force[3];
	double angular_rate[3];
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		specific_force[i] = accel_mps2[i] - filter->accel_bias_mps2[i];
		angular_rate[i] = gyro_rps[i] - filter->gyro_bias_rps[i];
		filter->last_unbiased_gyro_rps[i] = angular_rate[i];
	}

	double rotation[3][3];
	quaternion_rotation_matrix(filter->body_to_nav_quaternion, rotation);
	double linear_accel[3] = {0.0, 0.0, -config->gravity_mps2};
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			linear_accel[row] += rotation[row][col] * specific_force[col];
		}
		filter->last_linear_accel_nav_mps2[row] = linear_accel[row];
		filter->position_m[row] += filter->velocity_mps[row] * dt_s +
		                           0.5 * linear_accel[row] * dt_s * dt_s;
		filter->velocity_mps[row] += linear_accel[row] * dt_s;
	}

	double rotation_vector[3] = {
		angular_rate[0] * dt_s,
		angular_rate[1] * dt_s,
		angular_rate[2] * dt_s
	};
	double delta_quaternion[4];
	double updated_quaternion[4];
	rotation_vector_quaternion(rotation_vector, delta_quaternion);
	quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
	                    updated_quaternion);
	memcpy(filter->body_to_nav_quaternion, updated_quaternion,
	       sizeof(updated_quaternion));
	if(!quaternion_normalize(filter->body_to_nav_quaternion))
	{
		return false;
	}

	double system[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		system[ESKF_POSITION_INDEX + i][ESKF_VELOCITY_INDEX + i] = 1.0;
		system[ESKF_ATTITUDE_INDEX + i][ESKF_GYRO_BIAS_INDEX + i] = -1.0;
	}
	double force_skew[3][3];
	double rate_skew[3][3];
	skew_matrix(specific_force, force_skew);
	skew_matrix(angular_rate, rate_skew);
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			double rotated_skew = 0.0;
			for(unsigned int k = 0 ; k < 3u ; k++)
			{
				rotated_skew += rotation[row][k] * force_skew[k][col];
			}
			system[ESKF_VELOCITY_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
				-rotated_skew;
			system[ESKF_VELOCITY_INDEX + row][ESKF_ACCEL_BIAS_INDEX + col] =
				-rotation[row][col];
			system[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] =
				-rate_skew[row][col];
		}
	}

	double transition[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			transition[row][col] = system[row][col] * dt_s;
		}
		transition[row][row] += 1.0;
	}

	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	double propagated[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
			{
				intermediate[row][col] +=
					transition[row][k] * filter->covariance[k][col];
			}
		}
	}
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
			{
				propagated[row][col] += intermediate[row][k] * transition[col][k];
			}
		}
	}

	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			double accel_noise = 0.0;
			for(unsigned int axis = 0 ; axis < 3u ; axis++)
			{
				accel_noise += rotation[row][axis] *
				               square(config->accel_noise_density[axis]) *
				               rotation[col][axis];
			}
			propagated[ESKF_VELOCITY_INDEX + row][ESKF_VELOCITY_INDEX + col] +=
				accel_noise * dt_s;
		}
		propagated[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + row] +=
			square(config->gyro_noise_density[row]) * dt_s;
		propagated[ESKF_ACCEL_BIAS_INDEX + row][ESKF_ACCEL_BIAS_INDEX + row] +=
			square(config->accel_bias_random_walk[row]) * dt_s;
		propagated[ESKF_GYRO_BIAS_INDEX + row][ESKF_GYRO_BIAS_INDEX + row] +=
			square(config->gyro_bias_random_walk[row]) * dt_s;
	}
	memcpy(filter->covariance, propagated, sizeof(propagated));
	covariance_symmetrize_and_floor(filter, config->covariance_diagonal_floor);
	filter->propagation_count++;
	return eskf_state_is_finite(filter);
}

bool eskf_propagate(eskf_t* filter,
	                const eskf_config_t* config,
	                const double accel_mps2[3],
	                const double gyro_rps[3],
	                double dt_s)
{
	if((filter == NULL) || !filter->initialized || (config == NULL) ||
	   !vector_is_finite(accel_mps2, 3u) || !vector_is_finite(gyro_rps, 3u) ||
	   !isfinite(dt_s) || (dt_s < 0.0) ||
	   !(config->max_propagation_step_s > 0.0))
	{
		return false;
	}
	if(dt_s == 0.0)
	{
		return true;
	}
	unsigned int steps = (unsigned int)ceil(dt_s / config->max_propagation_step_s);
	if((steps == 0u) || (steps > 10000u))
	{
		return false;
	}
	double step_s = dt_s / (double)steps;
	for(unsigned int step = 0 ; step < steps ; step++)
	{
		if(!eskf_propagate_step(filter, config, accel_mps2, gyro_rps, step_s))
		{
			return false;
		}
	}
	return true;
}

static bool eskf_inject_error(eskf_t* filter, const double error[ESKF_ERROR_STATE_DIM])
{
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		filter->position_m[i] += error[ESKF_POSITION_INDEX + i];
		filter->velocity_mps[i] += error[ESKF_VELOCITY_INDEX + i];
		filter->accel_bias_mps2[i] += error[ESKF_ACCEL_BIAS_INDEX + i];
		filter->gyro_bias_rps[i] += error[ESKF_GYRO_BIAS_INDEX + i];
	}
	double attitude_error[3] = {
		error[ESKF_ATTITUDE_INDEX],
		error[ESKF_ATTITUDE_INDEX + 1u],
		error[ESKF_ATTITUDE_INDEX + 2u]
	};
	double delta_quaternion[4];
	double updated_quaternion[4];
	rotation_vector_quaternion(attitude_error, delta_quaternion);
	quaternion_multiply(filter->body_to_nav_quaternion, delta_quaternion,
	                    updated_quaternion);
	memcpy(filter->body_to_nav_quaternion, updated_quaternion,
	       sizeof(updated_quaternion));
	if(!quaternion_normalize(filter->body_to_nav_quaternion))
	{
		return false;
	}

	/* Right attitude-error reset after q <- q (x) Exp(delta_theta). */
	double reset[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
	{
		reset[i][i] = 1.0;
	}
	double attitude_skew[3][3];
	skew_matrix(attitude_error, attitude_skew);
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			reset[ESKF_ATTITUDE_INDEX + row][ESKF_ATTITUDE_INDEX + col] -=
				0.5 * attitude_skew[row][col];
		}
	}
	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	double reset_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
			{
				intermediate[row][col] += reset[row][k] * filter->covariance[k][col];
			}
		}
	}
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
			{
				reset_covariance[row][col] += intermediate[row][k] * reset[col][k];
			}
		}
	}
	memcpy(filter->covariance, reset_covariance, sizeof(reset_covariance));
	return true;
}

static bool eskf_selected_update(eskf_t* filter,
	                             unsigned int selected_state_index,
	                             const double residual[3],
	                             const double measurement_covariance[9],
	                             bool gate_enabled,
	                             double gate,
	                             double* nis_out)
{
	if((selected_state_index + 3u) > ESKF_ERROR_STATE_DIM ||
	   !vector_is_finite(residual, 3u) ||
	   !vector_is_finite(measurement_covariance, 9u))
	{
		return false;
	}
	double covariance_inverse[9];
	if(!inverse_symmetric3(measurement_covariance, covariance_inverse))
	{
		return false;
	}
	double innovation[9];
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			innovation[row * 3u + col] =
				filter->covariance[selected_state_index + row][selected_state_index + col] +
				measurement_covariance[row * 3u + col];
		}
	}
	double innovation_inverse[9];
	if(!inverse_symmetric3(innovation, innovation_inverse))
	{
		return false;
	}
	double nis = 0.0;
	for(unsigned int row = 0 ; row < 3u ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			nis += residual[row] * innovation_inverse[row * 3u + col] * residual[col];
		}
	}
	if(nis_out != NULL)
	{
		*nis_out = nis;
	}
	if(!isfinite(nis) || (gate_enabled && (nis > gate)))
	{
		return false;
	}

	double gain[ESKF_ERROR_STATE_DIM][3] = {{0.0}};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			for(unsigned int k = 0 ; k < 3u ; k++)
			{
				gain[row][col] +=
					filter->covariance[row][selected_state_index + k] *
					innovation_inverse[k * 3u + col];
			}
		}
	}
	double error[ESKF_ERROR_STATE_DIM] = {0.0};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			error[row] += gain[row][col] * residual[col];
		}
	}

	double joseph_left[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		joseph_left[row][row] = 1.0;
		for(unsigned int col = 0 ; col < 3u ; col++)
		{
			joseph_left[row][selected_state_index + col] -= gain[row][col];
		}
	}
	double intermediate[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	double updated_covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
			{
				intermediate[row][col] +=
					joseph_left[row][k] * filter->covariance[k][col];
			}
		}
	}
	for(unsigned int row = 0 ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0 ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			for(unsigned int k = 0 ; k < ESKF_ERROR_STATE_DIM ; k++)
			{
				updated_covariance[row][col] +=
					intermediate[row][k] * joseph_left[col][k];
			}
			for(unsigned int first = 0 ; first < 3u ; first++)
			{
				for(unsigned int second = 0 ; second < 3u ; second++)
				{
					updated_covariance[row][col] += gain[row][first] *
						measurement_covariance[first * 3u + second] * gain[col][second];
				}
			}
		}
	}
	memcpy(filter->covariance, updated_covariance, sizeof(updated_covariance));
	if(!eskf_inject_error(filter, error))
	{
		return false;
	}
	covariance_symmetrize_and_floor(filter, 1.0e-12);
	return eskf_state_is_finite(filter);
}

eskf_update_result_t eskf_position_update(
	eskf_t* filter,
	const eskf_config_t* config,
	const double position_m[3],
	const double covariance_m2[9],
	double* nis_out)
{
	if(nis_out != NULL)
	{
		*nis_out = NAN;
	}
	if((filter == NULL) || !filter->initialized || (config == NULL) ||
	   !vector_is_finite(position_m, 3u) || !vector_is_finite(covariance_m2, 9u))
	{
		return ESKF_UPDATE_INVALID;
	}
	double residual[3];
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		residual[i] = position_m[i] - filter->position_m[i];
	}
	double nis = NAN;
	if(!eskf_selected_update(filter, ESKF_POSITION_INDEX, residual, covariance_m2,
	                         true, config->position_nis_gate, &nis))
	{
		filter->last_position_nis = nis;
		if(isfinite(nis) && (nis > config->position_nis_gate))
		{
			filter->position_update_rejected_count++;
			if(nis_out != NULL)
			{
				*nis_out = nis;
			}
			return ESKF_UPDATE_REJECTED_NIS;
		}
		return ESKF_UPDATE_INVALID;
	}
	filter->last_position_nis = nis;
	filter->position_update_accepted_count++;
	if(nis_out != NULL)
	{
		*nis_out = nis;
	}
	return ESKF_UPDATE_ACCEPTED;
}

bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps)
{
	if((filter == NULL) || !filter->initialized ||
	   !(velocity_std_mps > 0.0) || !isfinite(velocity_std_mps))
	{
		return false;
	}
	double residual[3] = {
		-filter->velocity_mps[0],
		-filter->velocity_mps[1],
		-filter->velocity_mps[2]
	};
	double covariance[9] = {0.0};
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		covariance[i * 3u + i] = square(velocity_std_mps);
	}
	return eskf_selected_update(filter, ESKF_VELOCITY_INDEX, residual, covariance,
	                            false, 0.0, NULL);
}

bool eskf_zero_angular_rate_update(
	eskf_t* filter,
	const double measured_gyro_rps[3],
	const double measurement_std_rps[3])
{
	if((filter == NULL) || !filter->initialized ||
	   !vector_is_finite(measured_gyro_rps, 3u) ||
	   !vector_is_finite(measurement_std_rps, 3u))
	{
		return false;
	}
	double residual[3];
	double covariance[9] = {0.0};
	for(unsigned int i = 0 ; i < 3u ; i++)
	{
		if(!(measurement_std_rps[i] > 0.0))
		{
			return false;
		}
		residual[i] = measured_gyro_rps[i] - filter->gyro_bias_rps[i];
		covariance[i * 3u + i] = square(measurement_std_rps[i]);
	}
	return eskf_selected_update(filter, ESKF_GYRO_BIAS_INDEX, residual, covariance,
	                            false, 0.0, NULL);
}

double eskf_speed_mps(const eskf_t* filter)
{
	if((filter == NULL) || !filter->initialized)
	{
		return NAN;
	}
	return vector_norm3(filter->velocity_mps);
}

bool eskf_state_is_finite(const eskf_t* filter)
{
	if((filter == NULL) || !filter->initialized ||
	   !vector_is_finite(filter->position_m, 3u) ||
	   !vector_is_finite(filter->velocity_mps, 3u) ||
	   !vector_is_finite(filter->body_to_nav_quaternion, 4u) ||
	   !vector_is_finite(filter->accel_bias_mps2, 3u) ||
	   !vector_is_finite(filter->gyro_bias_rps, 3u) ||
	   !vector_is_finite(&filter->covariance[0][0],
	                     ESKF_ERROR_STATE_DIM * ESKF_ERROR_STATE_DIM))
	{
		return false;
	}
	for(unsigned int i = 0 ; i < ESKF_ERROR_STATE_DIM ; i++)
	{
		if(filter->covariance[i][i] < 0.0)
		{
			return false;
		}
	}
	return true;
}