int main()
{
FILE *fp;
float a1, a2;
double Aarray[] = { 1,1,-0.1,1 };
double Harray[] = { 1,1,0,1 };
Mat *X[100],*Qk,*Rk,*A,*H,*Z,*Pk,*K;
int iter = 0;
fopen_s(&fp, "measure.txt", "r");
Z = mallocMat(2, 1, TYPE_FLOAT);
X[0] = mallocMat(2, 1, TYPE_FLOAT);
setVecVal(X[0], 0, 0.1);
setVecVal(X[0], 1, 0.9);
A = scanMat(2, 2, TYPE_FLOAT, Aarray);
H = scanMat(2, 2, TYPE_FLOAT, Harray);
Qk = identity(2, TYPE_FLOAT);
Rk = identity(2, TYPE_FLOAT);
Pk = identity(2, TYPE_FLOAT);
K = zerosMat(2, 2, TYPE_FLOAT);
if (!fp)
{
return -1;
}
while (!feof(fp))
{
fscanf_s(fp, "%f %f", &a1, &a2);
if (iter == 0)
{
iter++;
continue;
}
setVecVal(Z, 0, a1);
setVecVal(Z, 1, a2);
X[iter] = mallocMat(2, 1, TYPE_FLOAT);
predictState(A, X[iter - 1], X[iter]);
predictErrorVariance(A, Pk, Qk, Pk);
if (iter % 5 == 0)
{
getK(Pk, H, Rk, K);
stateCorrect(X[iter], K, Z, H, X[iter]);
varianceCorrect(Pk, K, H, Pk);
}
iter++;
}
return 1;
}
bool Ekf::update()
{
bool ret = false; // indicates if there has been an update
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
//printStates();
//printStatesFast();
// prediction
if (_imu_updated) {
ret = true;
predictState();
predictCovariance();
}
// measurement updates
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
//fuseHeading();
fuseMag(_mag_fuse_index);
_mag_fuse_index = (_mag_fuse_index + 1) % 3;
}
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
_fuse_height = true;
}
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
} else if (_time_last_imu - _time_last_gps > 2000000 && _time_last_imu - _time_last_fake_gps > 70000) {
// if gps does not update then fake position and horizontal veloctiy measurements
_fuse_hor_vel = true; // we only fake horizontal velocity because we still have baro
_gps_sample_delayed.vel.setZero();
}
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
fuseRange();
}
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
fuseAirspeed();
}
calculateOutputStates();
return ret;
}
bool Ekf::update()
{
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();
// perform state and variance prediction for the terrain estimator
if (!_terrain_initialised) {
_terrain_initialised = initHagl();
} else {
predictHagl();
}
// control logic
controlFusionModes();
// measurement updates
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
fuseMag2D();
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();
} else {
// do no fusion at all
}
}
// determine if range finder data has fallen behind the fusin time horizon fuse it if we are
// not tilted too much to use it
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_prev(2, 2) > 0.7071f)) {
// if we have range data we always try to estimate terrain height
_fuse_hagl_data = true;
// only use range finder as a height observation in the main filter if specifically enabled
if (_control_status.flags.rng_hgt) {
_fuse_height = true;
}
} else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) {
// If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements
// and are on the ground, then synthesise a measurement at the expected on ground value
if (!_in_air) {
_range_sample_delayed.rng = _params.rng_gnd_clearance;
_range_sample_delayed.time_us = _imu_sample_delayed.time_us;
}
_fuse_height = true;
}
// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_control_status.flags.baro_hgt) {
_fuse_height = true;
} else {
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset;
_baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f);
}
}
// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) {
// Only use GPS data for position and velocity aiding if enabled
if (_control_status.flags.gps) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
}
// only use gps height observation in the main filter if specifically enabled
if (_control_status.flags.gps_hgt) {
_fuse_height = true;
}
}
// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
&& (_R_prev(2, 2) > 0.7071f)) {
_fuse_flow = true;
}
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!_control_status.flags.gps && !_control_status.flags.opt_flow
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
_fuse_pos = true;
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
_time_last_fake_gps = _time_last_imu;
}
// fuse available range finder data into a terrain height estimator if it has been initialised
if (_fuse_hagl_data && _terrain_initialised) {
fuseHagl();
_fuse_hagl_data = false;
}
// Fuse available NED velocity and position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
// Update optical flow bias estimates
calcOptFlowBias();
// Fuse optical flow LOS rate observations into the main filter
if (_fuse_flow) {
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_fuse_flow = false;
}
// TODO This is just to get the logic inside but we will only start fusion once we tested this again
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed) && false) {
fuseAirspeed();
}
}
// the output observer always runs
calculateOutputStates();
// check for NaN on attitude states
if (isnan(_state.quat_nominal(0)) || isnan(_output_new.quat_nominal(0))) {
return false;
}
// We don't have valid data to output until tilt and yaw alignment is complete
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
return true;
} else {
return false;
}
}
void KalmanFilter::updateMeasurement(double measurement, double variance,
const Eigen::Matrix<double, 1, 2>& C, double timestamp) {
updatePose(timestamp);
updateState(measurement, variance, C, predictState(timestamp));
lastMeasurementTimestamp_ = timestamp;
}
int
KalmanFilterSerial::startFilter()
{
// Nothing to do...
if (mTrack.track.empty ( )) {
PRINT_WARN ("Empty Track1!");
return -1;
}
KalmanFilterParameter_t kalmanParams;
size_t hitCount = mTrack.track.size ( );
kalmanParams.hits = (TrackHit_t*) calloc (hitCount, sizeof (TrackHit_t));
kalmanParams.fitsForward = (trackHitData*) calloc (hitCount, sizeof (trackHitData));
kalmanParams.fitsBackward = (trackHitData*) calloc (hitCount, sizeof (trackHitData));
kalmanParams.C_Values = (scalar_t*) calloc (hitCount, ORDER * ORDER * sizeof (scalar_t));
kalmanParams.C_Inverse = (scalar_t*) calloc (hitCount, ORDER * ORDER * sizeof (scalar_t));
for (size_t i = 0; i < hitCount; ++i) {
kalmanParams.hits[i] = mTrack.track.at(i);
}
GSL_VECTOR_SET_ZERO (mP_k1);
GSL_MATRIX_SET_ZERO (mC_k1);
GSL_MATRIX_SET_ZERO (mDim2_H);
// Initial error covariance matrix estimate
GSL_MATRIX_SET (mC_k1, 0, 0, 250.);
GSL_MATRIX_SET (mC_k1, 1, 1, 250.);
GSL_MATRIX_SET (mC_k1, 2, 2, 0.25);
GSL_MATRIX_SET (mC_k1, 3, 3, 0.25);
GSL_MATRIX_SET (mC_k1, 4, 4, 0.000001);
// Set the transport matrix H_(k)
GSL_MATRIX_SET (mDim1_H, 0, 0, 1.0);
GSL_MATRIX_SET (mDim2_H, 0, 0, 1.0);
GSL_MATRIX_SET (mDim2_H, 1, 1, 1.0);
for (size_t i = 0; i < hitCount; ++i) {
GSL_MATRIX_VIEW jacobi;
// Get the Transport-Jacobians
jacobi = GSL_MATRIX_VIEW_ARRAY (kalmanParams.hits[i].jacobi, 5, 5);
predictState (mP_k, &jacobi.matrix, mP_k1);
predictCovariance (mC_k, &jacobi.matrix, mC_k1);
#if HAS_MATERIAL_EFFECTS
scalar_t covVal = GSL_MATRIX_GET(mC_k, 2, 2);
GSL_MATRIX_SET(mC_k, 2, 2, covVal+kalmanParams.hits[i].sigmaDeltaPhiSquared);
covVal = GSL_MATRIX_GET(mC_k, 3, 3);
GSL_MATRIX_SET(mC_k, 3, 3, covVal+kalmanParams.hits[i].sigmaDeltaThetaSquared);
covVal = GSL_MATRIX_GET(mC_k, 4, 4);
GSL_MATRIX_SET(mC_k, 4, 4, covVal+kalmanParams.hits[i].sigmaDeltaQoverPSquared);
#endif
if (kalmanParams.hits[i].is2Dim) {
scalar_t value1, value2;
value1 = kalmanParams.hits[i].normal[0] - kalmanParams.hits[i].ref[0];
value2 = kalmanParams.hits[i].normal[1] - kalmanParams.hits[i].ref[1];
GSL_VECTOR_SET (mDim2_m, 0, value1);
GSL_VECTOR_SET (mDim2_m, 1, value2);
GSL_MATRIX_SET (mDim2_V, 0, 0, kalmanParams.hits[i].err_locX * kalmanParams.hits[i].err_locX);
GSL_MATRIX_SET (mDim2_V, 0, 1, kalmanParams.hits[i].cov_locXY);
GSL_MATRIX_SET (mDim2_V, 1, 0, kalmanParams.hits[i].cov_locXY);
GSL_MATRIX_SET (mDim2_V, 1, 1, kalmanParams.hits[i].err_locY * kalmanParams.hits[i].err_locY);
correctGain (mDim2_K, mC_k, mDim2_H, mDim2_V);
correctState (mP_k1, mP_k, mDim2_K, mDim2_m, mDim2_H);
correctCovariance (mC_k1, mDim2_K, mDim2_H, mC_k);
} else {
scalar_t value;
value = kalmanParams.hits[i].normal[0] - kalmanParams.hits[i].ref[0];
GSL_VECTOR_SET (mDim1_m, 0, value);
GSL_MATRIX_SET (mDim1_V, 0, 0, kalmanParams.hits[i].err_locX * kalmanParams.hits[i].err_locX);
correctGain (mDim1_K, mC_k, mDim1_H, mDim1_V);
correctState (mP_k1, mP_k, mDim1_K, mDim1_m, mDim1_H);
correctCovariance (mC_k1, mDim1_K, mDim1_H, mC_k);
}
/* TODO: implemenent
for (int c = 0; c < 5; ++c) {
for (int k = 0; k < 5; ++k)
kalmanParams.C_Values[c*5+k+i*25] = GSL_MATRIX_GET (mC_k1, c, k);
}
for (int c = 0; c < 5; ++c)
kalmanParams.fitsForward[i].data[c] = GSL_VECTOR_GET (mP_k1, c);
#if DBG_LVL > 0
std::cout << "P_k1: ";
printVector (mP_k1);
#endif
*/
}
/* TODO: implement
// Initial error covariance matrix estimate
GSL_MATRIX_SET (mC_k1, 0, 0, 250.);
GSL_MATRIX_SET (mC_k1, 1, 1, 250.);
GSL_MATRIX_SET (mC_k1, 2, 2, 0.25);
GSL_MATRIX_SET (mC_k1, 3, 3, 0.25);
GSL_MATRIX_SET (mC_k1, 4, 4, 0.000001);
for (int i = hitCount-1; i >= 0; --i) {
GSL_MATRIX_VIEW jacobi;
// Get the Transport-Jacobians
jacobi = GSL_MATRIX_VIEW_ARRAY (kalmanParams.hits[i].jacobiInverse, 5, 5);
predictState (mP_k, &jacobi.matrix, mP_k1);
predictCovariance (mC_k, &jacobi.matrix, mC_k1);
#if HAS_MATERIAL_EFFECTS
// add uncertainties from material effects:
scalar_t covVal = GSL_MATRIX_GET(mC_k, 2, 2);
GSL_MATRIX_SET(mC_k, 2, 2, covVal+kalmanParams.hits[i].sigmaDeltaPhiSquared);
covVal = GSL_MATRIX_GET(mC_k, 3, 3);
GSL_MATRIX_SET(mC_k, 3, 3, covVal+kalmanParams.hits[i].sigmaDeltaThetaSquared);
covVal = GSL_MATRIX_GET(mC_k, 4, 4);
GSL_MATRIX_SET(mC_k, 4, 4, covVal+kalmanParams.hits[i].sigmaDeltaQoverPSquared);
#endif
if (kalmanParams.hits[i].is2Dim) {
scalar_t value1, value2;
value1 = kalmanParams.hits[i].normal[0] - kalmanParams.hits[i].ref[0];
value2 = kalmanParams.hits[i].normal[1] - kalmanParams.hits[i].ref[1];
GSL_VECTOR_SET (mDim2_m, 0, value1);
GSL_VECTOR_SET (mDim2_m, 1, value2);
GSL_MATRIX_SET (mDim2_V, 0, 0, kalmanParams.hits[i].err_locX *
kalmanParams.hits[i].err_locX);
GSL_MATRIX_SET (mDim2_V, 0, 1, kalmanParams.hits[i].cov_locXY);
GSL_MATRIX_SET (mDim2_V, 1, 0, kalmanParams.hits[i].cov_locXY);
GSL_MATRIX_SET (mDim2_V, 1, 1, kalmanParams.hits[i].err_locY *
kalmanParams.hits[i].err_locY);
correctGain (mDim2_K, mC_k, mDim2_H, mDim2_V);
correctState (mP_k1, mP_k, mDim2_K, mDim2_m, mDim2_H);
correctCovariance (mC_k1, mDim2_K, mDim2_H, mC_k);
} else {
scalar_t value;
value = kalmanParams.hits[i].normal[0] - kalmanParams.hits[i].ref[0];
GSL_VECTOR_SET (mDim1_m, 0, value);
GSL_MATRIX_SET (mDim1_V, 0, 0, kalmanParams.hits[i].err_locX *
kalmanParams.hits[i].err_locX);
correctGain (mDim1_K, mC_k, mDim1_H, mDim1_V);
correctState (mP_k1, mP_k, mDim1_K, mDim1_m, mDim1_H);
correctCovariance (mC_k1, mDim1_K, mDim1_H, mC_k);
}
// FIXME: C_Inverse = C values im backward filter? (mit Rene klären)
for (int c = 0; c < 5; ++c) {
for (int k = 0; k < 5; ++k)
kalmanParams.C_Inverse[c*5+k+i*25] = GSL_MATRIX_GET (mC_k1, c, k);
}
for (int c = 0; c < 5; ++c)
kalmanParams.fitsBackward[i].data[c] = GSL_VECTOR_GET (mP_k1, c);
#if DBG_LVL > 0
std::cout << "P_k1: ";
printVector (mP_k1);
#endif
}
GSL_VECTOR *tmpVec;
tmpVec = GSL_VECTOR_CALLOC(5);
// Smoothing. FIXME: Save results to a useful struct..
for (size_t i = 0; i < hitCount; ++i) {
// Dim5_K = C_f * (C_f + C_b)^-1
GSL_MATRIX_VIEW C_f, C_b;
C_f = GSL_MATRIX_VIEW_ARRAY (&kalmanParams.C_Values[i*25], 5, 5);
C_b = GSL_MATRIX_VIEW_ARRAY (&kalmanParams.C_Inverse[i*25], 5, 5);
// mC_k = C_f + C_b
GSL_MATRIX_MEMCPY (mC_k, &C_f.matrix);
GSL_MATRIX_ADD (mC_k, &C_b.matrix);
GSL_MATRIX_MEMCPY (mInverse, mC_k);
// mInverse = (C_f + C_b)^-1
matrixInverseSymmetric (mInverse);
// mDim5_K = C_f * (C_f + C_b)^-1
GSL_BLAS_XGEMM (CblasNoTrans, CblasNoTrans, 1.0f, &C_f.matrix, mInverse, 0.0f, mDim5_K);
// P = p_f + K * p_b // SF: BUG!
// P = p_f + K * (p_b - p_f)
GSL_VECTOR_VIEW p_f, p_b;
p_f = GSL_VECTOR_VIEW_ARRAY (kalmanParams.fitsForward[i].data, 5);
p_b = GSL_VECTOR_VIEW_ARRAY (kalmanParams.fitsBackward[i].data, 5);
//GSL_BLAS_XGEMV (CblasNoTrans, 1.0, mDim5_K, &p_b.vector, 0.0, mP_k);
//tmpVec=p_b - p_f
GSL_VECTOR_MEMCPY(tmpVec, &p_b.vector);
GSL_VECTOR_SUB (tmpVec, &p_f.vector);
// mP_k = K * (p_b - p_f)
GSL_BLAS_XGEMV (CblasNoTrans, 1.0, mDim5_K, tmpVec, 0.0, mP_k);
// mP_k = p_f + K * (p_b - p_f)
GSL_VECTOR_ADD (mP_k, &p_f.vector);
#if BENCHMARK == 0
x->addWert (GSL_VECTOR_GET (mP_k, 0));
if (kalmanParams.hits[i].is2Dim)
y->addWert (GSL_VECTOR_GET (mP_k, 1));
#endif
// FIXME: calculation of smoothed covariance missing!
#if DBG_LVL > 2
std::cout << "C_f: ";
printMatrix (&C_f.matrix);
std::cout << "C_b: ";
printMatrix (&C_b.matrix);
std::cout << "p_f: ";
printVector (&p_f.vector);
std::cout << "p_b: ";
printVector (&p_b.vector);
std::cout << "SmoothedP: p_f + (C_f . invert(C_f + C_b)) . p_b;";
std::cout << "res: ";
printVector (mP_k);
#endif
#if DBG_LVL > 0
std::cout << "P_k: ";
printVector (mP_k);
#endif
}
*/
free (kalmanParams.hits);
free (kalmanParams.fitsForward);
free (kalmanParams.fitsBackward);
free (kalmanParams.C_Values);
free (kalmanParams.C_Inverse);
return 0;
}
