1 /* 2 * Copyright (C) 2016 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 ////////////////////////////////////////////////////////////////////////////// 18 /* 19 * This function implements a diversity checker and stores diverse vectors into 20 * a memory. We assume that the data is located on a sphere, and we use the 21 * norm of the difference of two vectors to decide if the vectors are diverse 22 * enough: 23 * 24 * k = norm( v1 - v2 )^2 < Threshold 25 * 26 * Hence when k < Threshold the data is not stored, because the vectors are too 27 * similar. We store diverse vectors in memory and all new incoming vectors 28 * are checked against the already stored data points. 29 * 30 * Furthermore we also check if k > max_distance, since that data is most likely 31 * not located on a sphere anymore and indicates a disturbance. Finally we give 32 * a "data is full" flag to indicate once the memory is full. 33 * The diverse data can be used to improve sphere fit calibrations, ensuring 34 * that the sphere is populated enough resulting in better fits. 35 * 36 * Memory is stored in an array initialized to length of 37 * [THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS], this has been done to be 38 * compatible with the full sphere fit algorithm. 39 * 40 * Notice, this function stops to check if data is diverse, once the memory is 41 * full. This has been done in order to save processing power. 42 */ 43 44 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_ 45 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_ 46 47 #include <stdbool.h> 48 #include <stddef.h> 49 #include <stdint.h> 50 51 #if defined(MAG_CAL_DEBUG_ENABLE) && !defined(DIVERSE_DEBUG_ENABLE) 52 // Ensures that diversity messaging is set when mag_cal debugging is enabled. 53 #define DIVERSE_DEBUG_ENABLE 54 #endif // MAG_CAL_DEBUG_ENABLE && !DIVERSE_DEBUG_ENABLE 55 56 #ifdef __cplusplus 57 extern "C" { 58 #endif 59 60 #define THREE_AXIS_DATA_DIM (3) // data is three-dimensional. 61 #define NUM_DIVERSE_VECTORS (30) // Storing 30 data points. 62 63 // Debug Messages 64 #ifdef DIVERSE_DEBUG_ENABLE 65 struct DiversityDbg { 66 uint32_t diversity_count; 67 float var_log; 68 float mean_log; 69 float max_log; 70 float min_log; 71 float diverse_data_log[THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS]; 72 size_t new_trigger; 73 }; 74 #endif 75 76 // DiversityChecker parameters container. 77 struct DiversityCheckerParameters { 78 float var_threshold; 79 float max_min_threshold; 80 float local_field; 81 float threshold_tuning_param; 82 float max_distance_tuning_param; 83 size_t min_num_diverse_vectors; 84 size_t max_num_max_distance; 85 }; 86 87 // Main data struct. 88 struct DiversityChecker { 89 // Data memory. 90 float diverse_data[THREE_AXIS_DATA_DIM * NUM_DIVERSE_VECTORS]; 91 92 // Number of data points in the memory. 93 size_t num_points; 94 95 // Number of data points that violated the max_distance condition. 96 size_t num_max_dist_violations; 97 98 // Threshold value that is used to check k against. 99 float threshold; 100 101 // Threshold tuning parameter used to calculate threshold (k_algo): 102 // threshold = threshold_tuning_param_sq * (local_field)^2. 103 float threshold_tuning_param_sq; 104 105 // Maximum distance value. 106 float max_distance; 107 108 // Max Distance tuning parameter: 109 // max_distance = max_distance_tuning_param_sq * (local_field)^2. 110 float max_distance_tuning_param_sq; 111 112 // Data full bit. 113 bool data_full; 114 115 // Setup variables for NormQuality check. 116 size_t min_num_diverse_vectors; 117 size_t max_num_max_distance; 118 float var_threshold; 119 float max_min_threshold; 120 121 // Debug Messages 122 #ifdef DIVERSE_DEBUG_ENABLE 123 struct DiversityDbg diversity_dbg; 124 #endif 125 }; 126 127 // Initialization of the function/struct, input parameters struct consists of: 128 // min_num_diverse_vectors -> sets the gate for a minimum number of data points 129 // in the memory 130 // max_num_max_distance -> sets the value for a max distance violation number 131 // gate. 132 // var_threshold -> is a threshold value for a Norm variance gate. 133 // max_min_threshold -> is a value for a gate that rejects Norm variations 134 // that are larger than this number. 135 // local_field -> is the assumed local_field (radius of the sphere). 136 // threshold_tuning_param -> threshold tuning parameter used to calculate 137 // threshold (k_algo). 138 // max_distance_tuning_param -> Max distance tuning parameter used to calculate 139 // max_distance. 140 void diversityCheckerInit(struct DiversityChecker* diverse_data, 141 const struct DiversityCheckerParameters* parameters); 142 143 // Resetting the memory and the counters, leaves threshold and max_distance 144 // as well as the setup variables for NormQuality check untouched. 145 void diversityCheckerReset(struct DiversityChecker* diverse_data); 146 147 // Checks if data point (x, y, z) is diverse against the diverse_data set. 148 // Returns true when the input point is diverse. 149 // Returns false when a maximum distance check is violated. 150 bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data, 151 float x, float y, float z); 152 153 // Main function. Tests the data (x,y,z) against the memory if diverse and 154 // stores it, if so. 155 void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, 156 float y, float z); 157 158 // Removing a constant bias from the diverse_data and check if the norm is 159 // within a defined bound: 160 // implemented 4 gates 161 // -> needs a minimum number of data points in the memory 162 // (controlled by min_num_divers_vectors). 163 // -> will return false if maximum number of max_distance is reached 164 // (controlled by max_num_max_distance). 165 // -> norm must be within a var window. 166 // -> norm must be within a MAX/MIN window. 167 // Returned value will only be true if all 4 gates are passed. 168 bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data, 169 float x_bias, float y_bias, float z_bias); 170 171 // This function updates the threshold value and max distance value based on the 172 // local field. This ensures a local field independent operation of the 173 // diversity checker. 174 // 175 // threshold = (threshold_tuning_param * local_field)^2 176 // max_distance = (max_distance_tuning_param * local_field)^2 177 void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data, 178 float local_field); 179 #ifdef __cplusplus 180 } 181 #endif 182 183 #endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_ 184