Matrix cholesky(Matrix P)
{
const double eps = 1e-6;
Matrix L = Matrix (P.getm(),P.getn(),false);
double a=0;
for(int i=0;i<P.getm();i++)
{
for(int j=0;j<i;j++)
{
a=P[i][j];
for(int k=0;k<j;k++)
{
a=a-L[i][k]*L[j][k];
}
L[i][j]=a/L[j][j];
}
a=P[i][i];
for(int k=0;k<i;k++)
{
a=a-pow(L[i][k],2);
}
if(a < 0)
{
// Due to rounding errors this can sometime become a small -ve number.
a = eps;
}
L[i][i]=sqrt(a);
}
return L;
}
Matrix CofactorMatrix(const Matrix& mat)
{
Matrix coMat(mat.getm(),mat.getn());
for(int i=0; i<mat.getm(); i++)
{
for(int j=0; j<mat.getn(); j++)
{
Matrix * minMat = new Matrix(mat.getm()-1, mat.getm()-1);
for(int k=0, l=0; k<mat.getn()-1; k++, l++)
{
if(k==i)
l++;
for(int m=0, n=0; m<mat.getn()-1; m++, n++)
{
if(m==j)
n++;
(*minMat)[k][m]=mat[l][n];
}
}
if((i+j)%2==0)
coMat[i][j]=determinant(*minMat);
else
coMat[i][j]=-determinant(*minMat);
delete minMat;
}
}
return coMat;
}
// Matrix Division by a Scalar
Matrix operator / (const Matrix& a, const double& b)
{
Matrix divAns(a.getm(),a.getn());
int i=0,j=0;
for (i=0; i<a.getm(); i++)
{
for (j=0; j<a.getn();j++)
{
divAns[i][j]=a[i][j]/b;
}
}
return divAns;
}
/*! @brief Sets the mean of the moment to the given value/s.
The dimensions of the new mean matrix must match the number of states for moment.
@param newMean the new value for the mean.
*/
void Moment::setMean(const Matrix& newMean)
{
assert(((unsigned int)newMean.getm() == m_numStates) && (newMean.getn() == 1));
bool isCorrectSize = ((unsigned int)newMean.getm() == m_numStates) && (newMean.getn() == 1);
assert(isCorrectSize);
assert(newMean.isValid());
if(isCorrectSize and newMean.isValid())
{
m_mean = newMean;
}
return;
}
// Matrix Multiplication by a Scalar
Matrix operator * (const double& a, const Matrix& b)
{
Matrix multAns(b.getm(),b.getn());
int i=0,j=0;
for (i=0; i<b.getm(); i++)
{
for (j=0; j<b.getn();j++)
{
multAns[i][j]=b[i][j]*a;
}
}
return multAns;
}
// Matrix Multiplication by a Scalar
Matrix operator * (const Matrix& a, const double& b)
{
Matrix multAns(a.getm(),a.getn());
int i=0,j=0;
for (i=0; i<a.getm(); i++)
{
for (j=0; j<a.getn();j++)
{
multAns[i][j]=a[i][j]*b;
}
}
return multAns;
}
Matrix InverseMatrix(const Matrix& mat)
{
if(mat.getm() == 1)
{
Matrix result(1,1,false);
result[0][0] = 1.f / mat[0][0];
return result;
}
else if(mat.getm() == 2)
return Invert22(mat);
else
return GaussJordanInverse(mat);
}
Matrix vertcat(Matrix a, Matrix b)
{
//Matrix concatenation
//assume same dimension on cols
int mTotal=a.getm()+b.getm();
Matrix c=Matrix(mTotal,a.getn(),false);
for(int i=0;i<a.getm();i++){
c.setRow(i,a.getRow(i));
}
for(int i=0;i<b.getm();i++){
c.setRow(i+a.getm(),b.getRow(i));
}
return c;
}
// Matrix Subtraction
Matrix operator - (const Matrix& a, const double& b)
{
Matrix subAns(a.getm(),a.getn());
int i=0,j=0;
for (i=0; i<a.getm(); i++)
{
for (j=0; j<a.getn();j++)
{
subAns[i][j]=a[i][j]-b;
}
}
return subAns;
}
bool SRUKF::setState(Matrix mean, Matrix sqrtCovariance)
{
if( (mean.getm() == sqrtCovariance.getm()) && (mean.getm() == sqrtCovariance.getn()) )
{
m_numStates = mean.getm();
m_mean = mean;
m_sqrtCovariance = sqrtCovariance;
CalculateSigmaWeights();
return true;
}
else
{
return false;
}
}
Matrix RobotModel::landmarkMeasurementEquation(const Matrix& state, const Matrix& measurementArgs)
{
// measurementArgs contain the vector [x,y]^T location of the object observed.
// Measurement is returned in polar coordinates [distance, theta]^T.
unsigned int total_objects = measurementArgs.getm() / 2;
Matrix expected_measurement(2*total_objects,1,false);
// variables required.
double dx,dy,distance,angle;
unsigned int current_index;
for (unsigned int object_number = 0; object_number < total_objects; ++object_number)
{
dx = measurementArgs[0][0] - state[kstates_x][0];
dy = measurementArgs[1][0] - state[kstates_y][0];
distance = sqrt(dx*dx + dy*dy);
angle = mathGeneral::normaliseAngle(atan2(dy,dx) - state[kstates_heading][0]);
// 2 measurements per object
current_index = 2*object_number;
// Write to matrix for return.
expected_measurement[current_index][0] = distance;
expected_measurement[current_index+1][0] = angle;
}
return expected_measurement;
}
Matrix<T> operator%(Matrix<T> &M)
{
try
{
if (_m != M.getm()) throw logic_error("Invalid matrix!");
int m = _m;
int n = _n + M.getn();
T** a = _mas;
T** b = M.getmas();
T** c = new T*[m];
for (int i = 0; i < m; i++)
c[i] = new T[n];
for (int i = 0; i < m; i++)
for (int j = 0; j < n; j++)
{
if (j < _n) c[i][j] = a[i][j];
else c[i][j] = b[i][j - _n];
}
Matrix<T> R(c, m, n);
for (int i = 0; i < m; i++)
delete[] c[i];
delete[] c;
return R;
}
catch (exception error)
{
cout << error.what() << endl;
}
}
Matrix<T> operator*(Matrix<T> &M)
{
try
{
if (_n != M.getm()) throw logic_error("Invalid matrix!");
int m = _m;
int n = _n;
int q = M.getn();
T** a = _mas;
T** b = M.getmas();
T** c = new T*[m];
for (int i = 0; i < m; i++)
c[i] = new T[q];
for (int i = 0; i < m; i++)
for (int j = 0; j < q; j++)
{
c[i][j] = 0;
for (int k = 0; k < n; k++)
c[i][j] += (a[i][k] * b[k][j]);
}
Matrix<T> R(c, m, q);
for (int i = 0; i < m; i++)
delete[] c[i];
delete[] c;
return R;
}
catch (exception error)
{
cout << error.what() << endl;
}
}
Matrix Matrix::operator = (const Matrix& B)
{
if (this == &B)
{
return *this;
}
else
{
delete[] data;
m = B.getm();
n = B.getn();
data = new double[m * n];
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
data[i * n + j] = B.get(i, j);
}
}
return *this;
}
}
// Matrix Subtraction
Matrix operator - (const Matrix& a, const Matrix& b)
{
Matrix subAns(a.getm(),a.getn());
int i=0,j=0;
if ((a.getn()==b.getn())&&(a.getm()==b.getm()))
{
for (i=0; i<a.getm(); i++)
{
for (j=0; j<a.getn();j++)
{
subAns[i][j]=a[i][j]-b[i][j];
}
}
}
return subAns;
}
double dot(const Matrix& mat1, const Matrix& mat2)
{
double ret = 0;
for(int i = 0; i<mat1.getm(); i++)
ret+=mat1[i][0]*mat2[i][0];
return ret;
}
void multijk(Matrix& A, Matrix& B, Matrix& C){
int l = A.getm();
int m = A.getn();
int n = B.getn();
for(int i=0; i < l; i++)
for(int j=0; j < m; j++)
for(int k=0; k < n; k++)
C(i,k) += A(i,j)*B(j,k);
}
Matrix diagcat(Matrix a, Matrix b)
{
//Matrix concatenation
//assume both Matrix a and b are square
int mTotal=a.getm()+b.getm();
int nTotal=a.getn()+b.getn();
Matrix c=Matrix(mTotal,nTotal,false);
for(int i=0;i<a.getm();i++){
for(int j=0;j<a.getn();j++){
c[i][j]=a[i][j];
}
}
for(int i=0;i<b.getm();i++){
for(int j=0;j<b.getn();j++){
c[i+a.getm()][j+a.getn()]=b[i][j];
}
}
return c;
}
/*! @brief Sets the covariance of the moment to the given value/s.
The dimensions of the new covariance matrix must match the number of states for moment.
@param newCovariance the new value for the covariance.
*/
void Moment::setCovariance(const Matrix& newCovariance)
{
bool isCorrectSize = (newCovariance.getm() == m_numStates) && (newCovariance.getn() == m_numStates);
assert(isCorrectSize);
assert(newCovariance.isValid());
if(isCorrectSize and newCovariance.isValid())
{
m_covariance = newCovariance;
}
return;
}
// 2x2 Matrix Inversion- t
Matrix Invert22(const Matrix& a)
{
Matrix invertAns(a.getm(),a.getn());
invertAns[0][0]=a[1][1];
invertAns[0][1]=-a[0][1];
invertAns[1][0]=-a[1][0];
invertAns[1][1]=a[0][0];
double divisor=a[0][0]*a[1][1]-a[0][1]*a[1][0];
invertAns=invertAns/divisor;
return invertAns;
}
// Matrix Addition
Matrix operator + (const Matrix& a, const Matrix& b)
{
Matrix addAns(a.getm(),a.getn());
int i=0,j=0;
if ((a.getn()==b.getn())&&(a.getm()==b.getm()))
{
for (i=0; i<a.getm(); i++)
{
for (j=0; j<a.getn();j++)
{
addAns[i][j]=a[i][j]+b[i][j];
}
}
}
return addAns;
//This return calls the copy constructor which copies the matrix into another block of memory
//and then returns the pointer to this new memory.
//Otherwise the array addAns is deleted by the destructor here and the pointer returned from the addition
//is a pointer to deleted memory. This causes problems when the function calling this tries to delete this memory again.
}
/*!
* @brief Performs the measurement update of the filter.
* @param measurement The measurement to be used for the update.
* @param measurementNoise The linear measurement noise that will be added.
* @param measurementArgs Any additional information about the measurement, if required.
* @return True if the measurement update was performed successfully. False if it was not.
*/
bool UKF::measurementUpdate(const Matrix& measurement, const Matrix& measurementNoise, const Matrix& measurementArgs)
{
const unsigned int totalPoints = totalSigmaPoints();
const unsigned int numStates = totalStates();
const unsigned int totalMeasurements = measurement.getm();
Matrix currentPoint; // temporary storage.
Matrix Yprop(totalMeasurements, totalPoints);
// First step is to calculate the expected measurmenent for each sigma point.
for (unsigned int i = 0; i < totalPoints; ++i)
{
currentPoint = m_sigma_points.getCol(i); // Get the sigma point.
Yprop.setCol(i, measurementEquation(currentPoint, measurementArgs));
}
// Now calculate the mean of these measurement sigmas.
Matrix Ymean = CalculateMeanFromSigmas(Yprop);
Matrix Pyy(measurementNoise); // measurement noise is added, so just use as the beginning value of the sum.
Matrix Pxy(numStates, totalMeasurements, false);
// Calculate the Pyy and Pxy variance matrices.
for(unsigned int i = 0; i < totalPoints; ++i)
{
double weight = m_covariance_weights[0][i];
// store difference between prediction and measurement.
currentPoint = Yprop.getCol(i) - Ymean;
// Innovation covariance - Add Measurement noise
Pyy = Pyy + weight * currentPoint * currentPoint.transp();
// Cross correlation matrix
Pxy = Pxy + weight * (m_sigma_points.getCol(i) - m_sigma_mean) * currentPoint.transp(); // Important: Use mean from estimate, not current mean.
}
// Calculate the Kalman filter gain
Matrix K;
// If we have a 2 dimensional measurement, use the faster shortcut function.
if(totalMeasurements == 2)
{
K = Pxy * Invert22(Pyy);
}
else
{
K = Pxy * InverseMatrix(Pyy);
}
Matrix newMean = mean() + K * (measurement - Ymean);
Matrix newCovariance = covariance() - K*Pyy*K.transp();
setMean(newMean);
setCovariance(newCovariance);
return true;
}
Matrix(Matrix<T> &M)
{
_m = M.getm();
_n = M.getn();
T** mas = M.getmas();
_mas = new T*[_m];
for (int i = 0; i < _m; i++)
_mas[i] = new T[_n];
for (int i = 0; i < _m; i++)
for (int j = 0; j < _n; j++)
_mas[i][j] = mas[i][j];
}
//Matrix Multiplication
Matrix operator * (const Matrix& a, const Matrix& b)
{
Matrix multAns(a.getm(),b.getn());
int i=0,j=0,k=0;
if (a.getn()==b.getm())
{
for (i=0; i<a.getm(); i++)
{
for (j=0; j<b.getn();j++)
{
double temp=0;
for (k=0; k<a.getn(); k++)
{
temp+=a[i][k]*b[k][j];
}
multAns[i][j]=temp;
}
}
}
else if (a.getm()==b.getm() && a.getn()==b.getn())
{
for (i=0; i<a.getm(); i++)
{
for (j=0; j<b.getn();j++)
{
multAns[i][j]=a[i][j]*b[i][j];
}
}
}
return multAns;
}
Matrix SRUKF::CalculateMeanFromSigmas(const Matrix& sigmaPoints) const
{
//unsigned int numPoints = sigmaPoints.getn();
Matrix mean(sigmaPoints.getm(),1,false);
mean = sigmaPoints * m_sigmaWeights.transp();
/*
for(unsigned int i = 0; i < numPoints; i++)
{
mean = mean + m_sigmaWeights[0][i]*sigmaPoints.getCol(i);
}
*/
return mean;
}
Matrix horzcat(Matrix a, Matrix b)
{
//Matrix concatenation
//assume same dimension on rows
int nTotal=a.getn()+b.getn();
Matrix c=Matrix(a.getm(),nTotal,false);
for(int i=0;i<a.getn();i++){
c.setCol(i,a.getCol(i));
}
for(int i=0;i<b.getn();i++){
c.setCol(i+a.getn(),b.getCol(i));
}
return c;
}
Matrix Matrix::setblock(int start_row, int start_column, Matrix& rhs)
{
int end_row = start_row + rhs.getm();
int end_column = start_column + rhs.getn();
for (int ii = start_row; ii < end_row; ii++)
{
for (int jj = start_column; jj < end_column; jj++)
{
double temp = rhs.get(ii - start_row, jj - start_column);
this->set(ii, jj, temp);
}
}
return rhs;
}
void WriteMatrix(std::ostream& out, const Matrix &mat)
{
int m = mat.getm(), n = mat.getn();
double element;
out.write(reinterpret_cast<const char*>(&m),sizeof(m));
out.write(reinterpret_cast<const char*>(&n),sizeof(n));
for(int i=0; i<m; i++)
{
for(int j=0; j<n; j++)
{
element = mat[i][j];
out.write(reinterpret_cast<const char*>(&element),sizeof(element));
}
}
return;
}
Matrix GaussJordanInverse(const Matrix& mat)
{
// Augment matrix with I - [mat | I]
Matrix A = horzcat(mat, Matrix(mat.getm(), mat.getn(), true));
int i,j;
unsigned int i_max = 0;
int k = 0;
double C;
for (k = 0; k < A.getm(); ++k)
{
// find max pivot.
i_max = k;
for (i = k; i < A.getm(); ++i)
{
if(fabs(A[i_max][k]) < fabs(A[i][k]))
i_max = i;
}
// Check if singular
if(A[i_max][k] == 0)
std::cout << "Matrix is singular" << std::endl;
A.swapRows(k, i_max);
for(i = k+1; i < A.getm(); ++i)
{
C = A[i][k] / A[k][k];
for(j = k+1; j < A.getn(); ++j)
{
A[i][j] -= A[k][j] * C;
}
A[i][k] = 0;
}
}
// Back substitution
for(k = A.getm()-1; k >= 0; --k)
{
C = A[k][k];
for(i=0; i < k; ++i)
{
for(j=A.getn()-1; j > k-1; --j)
{
A[i][j] -= A[k][j] * A[i][k] / C;
}
}
A.setRow(k, A.getRow(k) / C);
}
// Un-Augment matrix from I - [I | result]
Matrix result(mat.getm(), mat.getn(), false);
for(i=0; i < mat.getn(); ++i)
result.setCol(i, A.getCol(i + mat.getn()));
return result;
}
Matrix HT(Matrix A)
{
//Householder Triangularization Algorithm
//rows = n in the algorithm, for avoid confusion.
int rows=A.getm();
int r=A.getn()-rows;
double sigma;
double a;
double b;
std::vector <double> v (A.getn());
Matrix B= Matrix(rows,rows,false);
for(int k=rows-1;k>=0;k--){
sigma=0.0;
for(int j=0;j<=r+k;j++){
sigma=sigma+A[k][j]*A[k][j];
}
a=sqrt(sigma);
sigma=0.0;
for(int j=0;j<=r+k;j++){
if(j==r+k){
v.at(j)=(A[k][j]-a);
}
else{
v.at(j)=(A[k][j]);
}
sigma=sigma+v.at(j)*v.at(j);
}
a=2.0/(sigma+1e-15);
for(int i=0;i<=k;i++){
sigma=0.0;
for(int j=0;j<=r+k;j++){
sigma=sigma+A[i][j]*v.at(j);
}
b=a*sigma;
for(int j=0;j<=r+k;j++){
A[i][j]=A[i][j]-b*v.at(j);
}
}
}
for(int i=0;i<rows;i++){
B.setCol(i,A.getCol(r+i));
}
return B;
}
