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;
}
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);
}
/*! @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 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;
}
// 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 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 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;
}
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<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;
}
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<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;
}
}
bool SRUKF::measurementUpdate(const Matrix& measurement, const Matrix& measurementNoise, const Matrix& predictedMeasurementSigmas, const Matrix& stateEstimateSigmas)
{
int numberOfSigmaPoints = stateEstimateSigmas.getn();
debug << "Predicted measurement sigmas:" << std::endl << predictedMeasurementSigmas;
Matrix predictedMeasurement = CalculateMeanFromSigmas(predictedMeasurementSigmas);
Matrix Mz(m_numStates,numberOfSigmaPoints, false);
Matrix Mx(m_numStates,numberOfSigmaPoints, false);
for(int i = 0; i < numberOfSigmaPoints; i++)
{
Mz.setCol(i, m_sqrtSigmaWeights[0][i] * (predictedMeasurementSigmas.getCol(i) - predictedMeasurement));
Mx.setCol(i, m_sqrtSigmaWeights[0][i] * (stateEstimateSigmas.getCol(i) - m_mean));
}
Matrix Sz = horzcat(Mz,measurementNoise);
Matrix Pxz = Mx*Mz.transp();
Matrix K = Pxz * Invert22(Sz*Sz.transp());
debug << "K:" << std::endl << K;
m_mean = m_mean + K * (measurement - predictedMeasurement);
m_sqrtCovariance = HT(horzcat(Mx-K*Mz,K*measurementNoise));
//m_covariance = HT(horzcat(sigmaPoints-m_mean*m_sigmaWeights - K*predictedObservationSigmas +
// K*predictedObservation*m_sigmaWeights,K*measurementNoise));
return true;
}
//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 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;
}
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;
}
// 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 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;
}
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];
}
void locWmGlDisplay::DrawModel(const KF& model)
{
drawRobot(QColor(255,255,255,model.alpha*255), model.getState(KF::selfX), model.getState(KF::selfY), model.getState(KF::selfTheta));
if(drawSigmaPoints)
{
Matrix sigmaPoints = model.CalculateSigmaPoints();
for (int i=1; i < sigmaPoints.getn(); i++)
{
DrawSigmaPoint(QColor(255,255,255,model.alpha*255), sigmaPoints[KF::selfX][i], sigmaPoints[KF::selfY][i], sigmaPoints[KF::selfTheta][i]);
}
}
drawBall(QColor(255,165,0,255), model.getState(KF::ballX), model.getState(KF::ballY));
}
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 diag(const Matrix& A)
{
Matrix result;
if(A.getm() == 1)
{
unsigned int size = A.getn();
result = Matrix(size, size, false);
for(unsigned int i = 0; i < size; ++i)
{
result[i][i] = A[0][i];
}
}
else if(A.getn() == 1)
{
unsigned int size = A.getn();
result = Matrix(size, size, false);
for(unsigned int i = 0; i < size; ++i)
{
result[i][i] = A[i][0];
}
}
return result;
}
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 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 SRUKF::CalculateCovarianceFromSigmas(const Matrix& sigmaPoints, const Matrix& mean) const
{
unsigned int numPoints = sigmaPoints.getn();
Matrix diff;
Matrix temp(m_numStates, numPoints, false);
//debug << "mean" << std::endl << mean << std::endl;
for(unsigned int i = 0; i < numPoints; i++)
{
//debug << "sigma" << std::endl << sigmaPoints.getCol(i) << std::endl;
diff = sigmaPoints.getCol(i) - mean;
//debug << "diff" << std::endl << diff << std::endl;
temp.setCol(i, m_sqrtSigmaWeights[0][i] * diff);
}
debug << "temp" << std::endl << temp << std::endl;
return HT(temp);
}
bool Matrix::operator ==(const Matrix& b) const
{
float local_m = getm();
float local_n = getn();
// first compare dimensions
if(local_m != b.getm()) return false;
if(local_n != b.getn()) return false;
// now compare each value.
for(unsigned int i=0;i<local_m;++i)
{
for(unsigned int j=0;j<local_n;j++)
{
if( (*this)[i][j] != b[i][j])
{
return false;
}
}
}
return true;
}
