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; }