onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
f16ac8aa80ae75c877779a7928ab72bbae47538b
gps/soc/software/gps_system/src/eskf.c
T

810 lines
23 KiB
C
Raw Blame History

#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;
}
Reference in New Issue View Git Blame Copy Permalink