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