Commit ed4db91d authored by Ian McInerney's avatar Ian McInerney

Localization algorithm improvments

parent 5ea2e072
......@@ -35,6 +35,8 @@
#include "mac.h"
#include "debug.h"
#include "stabilizer_types.h"
#ifdef ESTIMATOR_TYPE_kalman
#include "estimator_kalman.h"
......@@ -51,7 +53,7 @@
#define RX_TIMEOUT 1000
#define TDMA_NSLOTS_BITS 2
#define TDMA_SLOT 1
//#define TDMA_SLOT 3
// TDMA handling
bool tdmaSynchronized;
......@@ -62,10 +64,14 @@ dwTime_t frameStart;
#define TDMA_NSLOTS_BITS 1
#endif
/*
#ifndef TDMA_SLOT
#warning "Slot number for TDMA not defined! Defaulting to 0."
#define TDMA_SLOT 0
#endif
*/
int TDMA_SLOT = 0;
#define TDMA_SLOT_BITS 27
......@@ -82,7 +88,7 @@ dwTime_t frameStart;
#define N_ANCHORS 6
int anchors[N_ANCHORS] = {1,2,3,4,5,6};
#define TAG_ADDRESS 8+TDMA_SLOT
int TAG_ADDRESS = 8;
// Outlier rejection
#ifdef COMPUTATION_TYPE_localization
......@@ -105,7 +111,7 @@ int anchors[N_ANCHORS] = {1,2,3,4,5,6};
} rangingStats[N_ANCHORS];
#endif
static uint8_t tag_address[] = {TAG_ADDRESS,0,0,0,0,0,0xcf,0xbc};
static uint8_t tag_address[] = {8,0,0,0,0,0,0xcf,0xbc};
static uint8_t base_address[] = {0,0,0,0,0,0,0xcf,0xbc};
// The four packets for ranging
......@@ -145,6 +151,12 @@ static bool ranging_complete = false;
static lpsAlgoOptions_t* options;
void setTDMAslot(int slotNumber) {
TDMA_SLOT = slotNumber;
TAG_ADDRESS = 8 + TDMA_SLOT;
tag_address[0] = TAG_ADDRESS;
}
static void txcallback(dwDevice_t *dev)
{
dwTime_t departure;
......
......@@ -15,6 +15,7 @@ void computation_packetReceived( CRTPPacket *pk );
void computation_processing();
void computation_algorithm_init();
void computation_algorithm_reset();
void computation_update_state( state_t *state, sensorData_t *sensor );
#endif
\ No newline at end of file
......@@ -9,7 +9,7 @@ typedef struct computation_localization_config_packet {
float k1;
float k2;
float k3;
} computation_localization_config_packet;
} __attribute__((packed)) computation_localization_config_packet ;
typedef struct computation_localization_shared_node_data {
float measuredRadius;
......@@ -19,7 +19,7 @@ typedef struct computation_localization_shared_node_data {
float alpha;
float beta;
float gamma;
} computation_localization_node_data;
} __attribute__((packed)) computation_localization_node_data;
void computation_localization_newDistance(int anchor, float newRange);
......
......@@ -47,6 +47,7 @@ static void Computation_crtpCB(CRTPPacket* pk) {
// Toggle the computation on or off
computationEnabled = pk->data[1];
if (computationEnabled) {
computation_algorithm_reset();
DEBUG_PRINT("Enabled Computation\n");
} else {
DEBUG_PRINT("Disabled Computation\n");
......@@ -90,6 +91,8 @@ void Computation_Init() {
crtpInit();
crtpRegisterPortCB(CRTP_PORT_COMP, Computation_crtpCB);
computation_algorithm_init();
// Create the computation task
xTaskCreate(Computation_Task, COMPUTATION_TASK_NAME,
COMPUTATION_TASK_STACKSIZE, NULL, COMPUTATION_TASK_PRI, NULL);
......
......@@ -33,50 +33,115 @@ static float stepSize;
static uint8_t numNodes;
static uint8_t nodeNumber;
static float dmu;
static float da;
static float dx;
static float dy;
static float dz;
static float dalpha;
static float dbeta;
static float dgamma;
// This is the data from the neighbors
static computation_localization_node_data nodeData[MAX_NUM_NODES];
static uint8_t useNode[MAX_NUM_NODES];
static uint32_t lastNodeTime[MAX_NUM_NODES];
extern void setTDMAslot(int slotNumber);
/*
* This function will configure the algorithm to be in its default state
*/
void computation_algorithm_init() {
// Zero out the stuff for this node
a = 0;
mu = 0;
localData.estimatedX = 0;
localData.estimatedY = 0;
localData.estimatedZ = 0;
localData.alpha = 0;
localData.beta = 0;
localData.gamma = 0;
localPosition.x = 0;
localPosition.y = 0;
localPosition.z = 0;
twok1 = 0;
twok2 = 0;
twok3 = 0;
stepSize = 0;
a = 0.0;
mu = 0.0;
localData.measuredRadius = 0.0;
localData.estimatedX = 0.0;
localData.estimatedY = 0.0;
localData.estimatedZ = 0.0;
localData.alpha = 0.0;
localData.beta = 0.0;
localData.gamma = 0.0;
localPosition.x = 0.0;
localPosition.y = 0.0;
localPosition.z = 0.0;
dmu = 0;
da = 0;
dx = 0;
dy = 0;
dz = 0;
dalpha = 0;
dbeta = 0;
dgamma = 0;
twok1 = 0.0;
twok2 = 0.0;
twok3 = 0.0;
stepSize = 0.0;
numNodes = 0;
// Zero out the ranges array
for (uint8_t i = 0; i < LOCODECK_NR_OF_ANCHORS; i++) {
ranges[i] = 0.0;
}
// Zero out the stuff from the other nodes
for (uint8_t i = 0; i < MAX_NUM_NODES; i++) {
useNode[i] = 0;
lastNodeTime[i] = 0;
lastNodeTime[i] = 0.0;
nodeData[i].estimatedX = 0;
nodeData[i].estimatedY = 0;
nodeData[i].estimatedZ = 0;
nodeData[i].alpha = 0;
nodeData[i].beta = 0;
nodeData[i].gamma = 0;
nodeData[i].measuredRadius = 0.0;
nodeData[i].estimatedX = 0.0;
nodeData[i].estimatedY = 0.0;
nodeData[i].estimatedZ = 0.0;
nodeData[i].alpha = 0.0;
nodeData[i].beta = 0.0;
nodeData[i].gamma = 0.0;
}
}
void computation_algorithm_reset() {
// Create the initial conditions
a = -1.0 * ranges[anchorNumber];
localData.measuredRadius = ranges[anchorNumber];
localData.estimatedX = localPosition.x;
localData.estimatedY = localPosition.y;
localData.estimatedZ = localPosition.z;
// Zero out the dual variables
mu = 0.0;
localData.alpha = 0.0;
localData.beta = 0.0;
localData.gamma = 0.0;
// Zero the derivative terms
dmu = 0;
da = 0;
dx = 0;
dy = 0;
dz = 0;
dalpha = 0;
dbeta = 0;
dgamma = 0;
// Zero out the stuff from the other nodes
for (uint8_t i = 0; i < MAX_NUM_NODES; i++) {
useNode[i] = 0;
lastNodeTime[i] = 0.0;
nodeData[i].measuredRadius = 0.0;
nodeData[i].estimatedX = 0.0;
nodeData[i].estimatedY = 0.0;
nodeData[i].estimatedZ = 0.0;
nodeData[i].alpha = 0.0;
nodeData[i].beta = 0.0;
nodeData[i].gamma = 0.0;
}
}
......@@ -84,7 +149,6 @@ void computation_algorithm_init() {
* This is used to get the distance from the ranging deck.
*/
void computation_localization_newDistance(int anchor, float newRange) {
DEBUG_PRINT("D: %f\n", newRange);
ranges[anchor] = newRange;
}
......@@ -108,9 +172,11 @@ void computation_configReceived( CRTPPacket *pk, CRTPPacket *p ) {
nodeNumber = config.nodeNumber;
numNodes = config.numTotalNodes;
anchorNumber = config.anchor;
twok1 = 2*config.k1;
twok2 = 2*config.k2;
twok3 = 2*config.k3;
twok1 = 2.0*config.k1;
twok2 = 2.0*config.k2;
twok3 = 2.0*config.k3;
setTDMAslot(config.nodeNumber);
DEBUG_PRINT("COMP: Step %2.2f\n", stepSize);
DEBUG_PRINT("COMP: Node %d\n", nodeNumber);
......@@ -140,8 +206,8 @@ void computation_packetReceived( CRTPPacket *pk ) {
* This function does the actual computation
*/
void computation_processing() {
// The number of nodes used in this computation
uint8_t numComputingNodes = 0;
// The number of nodes in the computation
float numComputingNodes = 0;
// Go through and make sure all the data is valid
uint32_t currentTick = xTaskGetTickCount();
......@@ -152,48 +218,50 @@ void computation_processing() {
DEBUG_PRINT("Node %d has gone stale\n", i);
}
numComputingNodes += useNode[i];
numComputingNodes += (float) 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 - localData.measuredRadius) * (a - localData.measuredRadius);
float diffX = (localData.estimatedX - localPosition.x);
float diffY = (localData.estimatedY - localPosition.y);
float diffZ = (localData.estimatedZ - localPosition.z);
float diffA = (a - localData.measuredRadius);
// This term is needed by the others
dmu = (diffX * diffX ) + (diffY * diffY) + (diffZ * diffZ) - (diffA * diffA);
double da = 2 * (2*dmu + mu)*(a - localData.measuredRadius) - 2*a;
float pd = (2.0*dmu + mu);
da = 2.0 * pd * diffA - 2.0*a;
// These are based upon data from the other nodes
double dx = -2 * (2*dmu + mu)*(localData.estimatedX - localPosition.x)
+ numComputingNodes*localData.alpha
+ twok1 * numComputingNodes*localData.estimatedX;
double dy = -2 * (2*dmu + mu)*(localData.estimatedY - localPosition.y)
+ numComputingNodes*localData.beta
+ twok2 * numComputingNodes*localData.estimatedY;
double dz = -2 * (2*dmu + mu)*(localData.estimatedZ - localPosition.z)
+ numComputingNodes*localData.gamma
+ twok3 * numComputingNodes*localData.estimatedZ;
dx = -2.0 * pd * diffX
- numComputingNodes*localData.alpha
- twok1 * numComputingNodes*localData.estimatedX;
dy = -2.0 * pd * diffY
- numComputingNodes*localData.beta
- twok2 * numComputingNodes*localData.estimatedY;
dz = -2.0 * pd * diffZ
- numComputingNodes*localData.gamma
- twok3 * numComputingNodes*localData.estimatedZ;
double dalpha = -1 * numComputingNodes*localData.estimatedX;
double dbeta = -1 * numComputingNodes*localData.estimatedY;
double dgamma = -1 * numComputingNodes*localData.estimatedZ;
dalpha = numComputingNodes*localData.estimatedX;
dbeta = numComputingNodes*localData.estimatedY;
dgamma = numComputingNodes*localData.estimatedZ;
// Go through and compute the updates
for (int i = 0; i < MAX_NUM_NODES; i++ ) {
// Make sure the node should be used
if (useNode[i] == 0) {
continue;
if (useNode[i] != 0) {
dx += ( nodeData[i].alpha + twok1*nodeData[i].estimatedX );
dy += ( nodeData[i].beta + twok2*nodeData[i].estimatedY );
dz += ( nodeData[i].gamma + twok3*nodeData[i].estimatedZ );
dalpha -= nodeData[i].estimatedX;
dbeta -= nodeData[i].estimatedY;
dgamma -= nodeData[i].estimatedZ;
}
dx -= ( nodeData[i].alpha + twok1*nodeData[i].estimatedX );
dy -= ( nodeData[i].beta + twok1*nodeData[i].estimatedY );
dz -= ( nodeData[i].gamma + twok1*nodeData[i].estimatedZ );
dalpha += nodeData[i].estimatedX;
dbeta += nodeData[i].estimatedY;
dgamma += nodeData[i].estimatedZ;
}
// Update the local information (simple Euler's method)
......
......@@ -128,11 +128,12 @@ static void stabilizerTask(void* param)
#else
sensorsAcquire(&sensorData, tick);
stateEstimator(&state, &sensorData, tick);
computation_update_state(&state, &sensorData);
commanderXYZ_GetPosition(&state, tick);
#endif
commanderXYZ_GetSetpoint(&setpoint, &state, &resetControllers);
computation_update_state(&state, &sensorData);
#ifndef BYPASS_CONTROLLERS
stateController(&control, &sensorData, &state, &setpoint, tick, resetControllers);
......
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