/*Performs the kalman filter update step
* @param newState only continues position informations, velocity and yaw rate has to be computed first
*/
void PointCell::update(Matrix<double> newState)
{
double velocity, phi;
double xNew = newState.get(0,0);
double yNew = newState.get(1,0);
double thetaNew = newState.get(2,0);
double x = stateCopy.get(0,0);
double y = stateCopy.get(1,0);
double theta = stateCopy.get(2,0);
velocity = sqrt((xNew - x) * (xNew - x) + (yNew - y)*(yNew - y)) / TIMESTAMP;
phi = (thetaNew-theta) / TIMESTAMP;
// setX(xNew);
// setY(yNew);
// setTheta(thetaNew);
setVelocity(velocity);
setPhi(phi);
newState.put(3,0,velocity);
newState.put(4,0,phi);
Matrix<double> S = H*P*H.getTranspose() +R;
K = (P*H.getTranspose()) * S.getInverse();
stateVector = stateVector + (K*(newState - stateVector));
P = (I - (K*H))*P;
}
EulerAngles::EulerAngles(float phi, float theta, float psi) :
Vector(3)
{
setPhi(phi);
setTheta(theta);
setPsi(psi);
}
EulerAngles::EulerAngles() :
Vector(3)
{
setPhi(0.0f);
setTheta(0.0f);
setPsi(0.0f);
}
EulerAngles::EulerAngles(const Dcm &dcm) :
Vector(3)
{
setTheta(asinf(-dcm(2, 0)));
if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) + getPhi());
} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
setPhi(0.0f);
setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
dcm(0, 2) + dcm(1, 1)) - getPhi());
} else {
setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
}
}
AdaptiveSO2CPGSynPlas::AdaptiveSO2CPGSynPlas(Neuron* perturbingNeuron)
: ExtendedSO2CPG(perturbingNeuron){
setAlpha(1.01);
setPhi(0.3);
setMu(1.00);
setGamma(0.02);
setEpsilon(0.03);
setBeta(0.00);
// for a range of P from -1 to 1
setBetaDynamics (-1.0, 0.003, 0.0000);
setGammaDynamics (-1.0, 0.003, 1.0000);
setEpsilonDynamics(0.04, 0.003, 0.0001);
}
void AdaptiveSO2CPGSynPlas::updateWeights() {
const double& phi = getPhi();
const double& beta = getBeta();
const double& gamma = getGamma();
const double& epsilon = getEpsilon();
const double& mu = getMu();
const double& F = getOutput(2);
const double& w01 = getWeight(0,1);
const double& x = getOutput(0);
const double& y = getOutput(1);
const double& P = getPerturbation();
// general approach
setPhi ( phi + mu * gamma * F * w01 * y );
setBeta ( beta + betaHebbRate * x * F - betaDecayRate * (beta - betaZero) );
setGamma ( gamma + gammaHebbRate * x * F - gammaDecayRate * (gamma - gammaZero) );
setEpsilon( epsilon + epsilonHebbRate * F * P - epsilonDecayRate * (epsilon - epsilonZero) );
}
CCLib::SimpleCloud* ccGBLSensor::project(CCLib::GenericCloud* theCloud, int& errorCode, bool autoParameters/*false*/)
{
assert(theCloud);
CCLib::SimpleCloud* newCloud = new CCLib::SimpleCloud();
unsigned pointCount = theCloud->size();
if (!newCloud->reserve(pointCount) || !newCloud->enableScalarField()) //not enough memory
{
errorCode = -4;
delete newCloud;
return 0;
}
ScalarType maxDepth = 0;
theCloud->placeIteratorAtBegining();
{
for (unsigned i=0; i<pointCount; ++i)
{
const CCVector3 *P = theCloud->getNextPoint();
CCVector2 Q;
ScalarType depth;
projectPoint(*P,Q,depth);
newCloud->addPoint(CCVector3(Q.x,Q.y,0));
newCloud->setPointScalarValue(i,depth);
if (i != 0)
maxDepth = std::max(maxDepth,depth);
else
maxDepth = depth;
}
}
if (autoParameters)
{
//dimensions = theta, phi, 0
PointCoordinateType bbMin[3],bbMax[3];
newCloud->getBoundingBox(bbMin,bbMax);
setTheta(bbMin[0],bbMax[0]);
setPhi(bbMin[1],bbMax[1]);
setSensorRange(maxDepth);
}
//clear previous Z-buffer
{
if (m_depthBuffer.zBuff)
delete[] m_depthBuffer.zBuff;
m_depthBuffer.zBuff=0;
m_depthBuffer.width=0;
m_depthBuffer.height=0;
}
//init new Z-buffer
{
int width = static_cast<int>(ceil((thetaMax-thetaMin)/deltaTheta));
int height = static_cast<int>(ceil((phiMax-phiMin)/deltaPhi));
if (width*height == 0 || std::max(width,height) > 2048) //too small or... too big!
{
errorCode = -2;
delete newCloud;
return 0;
}
unsigned zBuffSize = width*height;
m_depthBuffer.zBuff = new ScalarType[zBuffSize];
if (!m_depthBuffer.zBuff) //not enough memory
{
errorCode = -4;
delete newCloud;
return 0;
}
m_depthBuffer.width = width;
m_depthBuffer.height = height;
memset(m_depthBuffer.zBuff,0,zBuffSize*sizeof(ScalarType));
}
//project points and accumulate them in Z-buffer
newCloud->placeIteratorAtBegining();
for (unsigned i=0; i<newCloud->size(); ++i)
{
const CCVector3 *P = newCloud->getNextPoint();
ScalarType depth = newCloud->getPointScalarValue(i);
int x = static_cast<int>(floor((P->x-thetaMin)/deltaTheta));
int y = static_cast<int>(floor((P->y-phiMin)/deltaPhi));
ScalarType& zBuf = m_depthBuffer.zBuff[y*m_depthBuffer.width+x];
zBuf = std::max(zBuf,depth);
}
errorCode = 0;
return newCloud;
}
