Commit 5ea2e072 authored by Ian McInerney's avatar Ian McInerney

Added measured radius to packet for reporting

parent e656daf6
......@@ -12,6 +12,7 @@ typedef struct computation_localization_config_packet {
} computation_localization_config_packet;
typedef struct computation_localization_shared_node_data {
float measuredRadius;
float estimatedX;
float estimatedY;
float estimatedZ;
......
......@@ -155,13 +155,15 @@ void computation_processing() {
numComputingNodes += useNode[i];
}
localData.measuredRadius = ranges[anchorNumber];
// These are local to the node
double dmu = (localData.estimatedX - localPosition.x) * (localData.estimatedX - localPosition.x)
+ (localData.estimatedY - localPosition.y) * (localData.estimatedY - localPosition.y)
+ (localData.estimatedZ - localPosition.z) * (localData.estimatedZ - localPosition.z)
- (a - ranges[anchorNumber]) * (a - ranges[anchorNumber]);
- (a - localData.measuredRadius) * (a - localData.measuredRadius);
double da = 2 * (2*dmu + mu)*(a - ranges[anchorNumber]) - 2*a;
double da = 2 * (2*dmu + mu)*(a - localData.measuredRadius) - 2*a;
// These are based upon data from the other nodes
double dx = -2 * (2*dmu + mu)*(localData.estimatedX - localPosition.x)
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment