/* This just does all the steps in order to complete a filter iteration. */
void UnscentedKalmanFilter::iterate(real_t dt, ControlVector c) {
create_sigma_points();
apriori_estimate(dt, c);
/* Check to make sure there's a need to run the measurement step. */
if(sensor.size() > 0) {
measurement_estimate();
calculate_innovation();
calculate_kalman_gain();
aposteriori_estimate();
} else {
state = sigma_points.col(0);
state_covariance = apriori_covariance;
}
if(state.attitude()(3) < 0) {
state.attitude() *= -1.0;
}
state.attitude().normalize();
}
void use_rnb_data(){
//uint32_t start = get_time();
uint8_t power = 255;
int16_t matrixSum = processBrightMeas();
float bearing, heading;
float error;
calculate_bearing_and_heading(&bearing, &heading);
float initial_range = get_initial_range_guess(bearing, heading, power);
if(initial_range!=0&&!isnanf(initial_range)){
float range = range_estimate(initial_range, bearing, heading, power);
if(!isnanf(range)){
if(range<2*DROPLET_RADIUS) range=5.0;
float fdR, fdB, fdH;
float conf = sqrtf(matrixSum);
fdB = bearing;
fdH = heading;
fdR = range;
rStep.f = 10*FD_INIT_STEP;
bStep.f = FD_INIT_STEP;
hStep.f = FD_INIT_STEP;
prevSgnEdR=0;
prevSgnEdB=0;
prevSgnEdH=0;
//print_brightMeas();
error = calculate_innovation(range, bearing, heading);
//printf("(RNB) ID: %04X \r\n\tBefore: % 5.1f, % 6.1f, % 6.1f\r\n", rnbCmdID, fdR, rad_to_deg(fdB), rad_to_deg(fdH));
//uint8_t i;
//start = get_time();
//uint8_t earlyAbort;
//float newR, newB, newH;
//for(i=0;i<15;i++){
//earlyAbort = finiteDifferenceStep(fdR, fdB, fdH, &newR, &newB, &newH);
////printf("\t\t% 5.1f, % 6.1f, % 6.1f | %6.4f, %6.4f\r\n", fdR, rad_to_deg(fdB), rad_to_deg(fdH), bStep.f, hStep.f);
//fdR = newR;
//fdB = newB;
//fdH = newH;
//if(earlyAbort) break;
//}
range = fdR;
bearing = fdB;
heading = fdH;
error = calculate_innovation(range, bearing, heading);
//printf("\t After: % 5.1f, % 6.1f, % 6.1f, %6.2f [%hu]\r\n", fdR, rad_to_deg(fdB), rad_to_deg(fdH), error>3.0 ? (conf/(10.0*error*error)) : (conf/(error*error)), i);
//printf("\tTook %lu ms.\r\n", get_time()-start);
conf = conf/(error*error);
if(error>3.0){
conf = conf/10.0; //Nerf the confidence hard if the calculated error was too high.
}
if(isnan(conf)){
conf = 0.01;
}
last_good_rnb.id_number = rnbCmdID;
last_good_rnb.range = range;
last_good_rnb.bearing = bearing;
last_good_rnb.heading = heading;
last_good_rnb.conf = conf;
rnb_updated=1;
}
}
ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
rnbProcessingFlag=0;
}
}