For whoever might need it. I had to create this matrix class for dealing with kalman filter. Since you helped me so much in this forum I soppuse that the final working code belongs to everyone.
Header file
Code:
#include <vector>
#include <iostream>
using namespace std;
class Matrix2_2;
class Vector1_2;
class Vector2_1
{
protected:
double a, b;
public:
Vector2_1();
Vector2_1(double vec1, double vec2);
void SetSpecificValue(double value, int pos);
double GetSpecificValue(int pos) const;
virtual void print();
Vector1_2 transpose() const;
Vector2_1 operator / (const double& C);
Matrix2_2 operator * (const Vector1_2& vec);
Vector2_1 operator * (const double& num);
Vector2_1 operator + (const Vector2_1&) const;
};
class Vector1_2:public Vector2_1
{
public:
Vector1_2():Vector2_1() {}
Vector1_2(double vec1, double vec2):Vector2_1(vec1, vec2) {}
Vector1_2 operator * (const Matrix2_2&) const;
double operator * (const Vector2_1& vec) const;
void print();
Vector2_1 transpose() const;
};
class Matrix2_2
{
private:
double M11, M12, M21, M22;
public:
Matrix2_2();
Matrix2_2(double UPLEFT, double UPRIGHT, double DOWNLEFT, double DOWNRIGHT):M11(UPLEFT), M12(UPRIGHT), M21(DOWNLEFT), M22(DOWNRIGHT) {}
Matrix2_2(const Matrix2_2& M2);
void SetAllValues(double UPLEFT, double UPRIGHT, double DOWNLEFT, double DOWNRIGHT);
void SetSpecificValue(double value, int row, int column);
double Getspecificvalue(int row, int column) const;
Matrix2_2 Transpose() const;
void print();
Matrix2_2 operator * (const Matrix2_2&) const;
Vector2_1 operator * (const Vector2_1&) const;
Matrix2_2 operator + (const Matrix2_2&) const;
Matrix2_2 operator - (const Matrix2_2&) const;
};
CPP
Code:
#include "windows.h"
#include "Matrix.h"
/////////////////VECTOR2_1//////////////////
Vector2_1::Vector2_1()
{a=0; b=0;}
Vector2_1::Vector2_1(double vec1, double vec2)
{a=vec1; b=vec2;}
void Vector2_1::SetSpecificValue(double value, int pos)
{
if(pos==1)
a=value;
else if(pos==2)
b=value;
else
MessageBox(NULL, "wrong way of dealing with Vector2_1 class", "Located in: void Vector2_1::SetSpecificValue", NULL);
}
double Vector2_1::GetSpecificValue(int pos) const
{
if(pos==1)
return a;
else if(pos==2)
return b;
else
MessageBox(NULL, "wrong way of dealing with Vector2_1 class", "Located in: void Vector2_1::SetSpecificValue", NULL);
return -99999; //error
}
void Vector2_1::print()
{cout<<endl<<"["<<a<<" "<<b<<"]T"<<endl;}
Vector1_2 Vector2_1::transpose() const
{
return Vector1_2(a,b);
}
Vector2_1 Vector2_1::operator / (const double& C)
{
return Vector2_1(a/C, b/C);
}
Matrix2_2 Vector2_1::operator * (const Vector1_2& vec)
{
return Matrix2_2(a*vec.GetSpecificValue(1), a*vec.GetSpecificValue(2), b*vec.GetSpecificValue(1), b*vec.GetSpecificValue(2));
}
Vector2_1 Vector2_1::operator * (const double& num)
{
return Vector2_1(a*num, b*num);
}
Vector2_1 Vector2_1::operator + (const Vector2_1& vec2) const
{
return Vector2_1(a+vec2.GetSpecificValue(1), b+vec2.GetSpecificValue(2));
}
/////////////////VECTOR1_2//////////////////
Vector1_2 Vector1_2::operator * (const Matrix2_2& M) const
{
Vector1_2 vec2;
vec2.SetSpecificValue(a*M.Getspecificvalue(1,1)+b*M.Getspecificvalue(2,1),1);
vec2.SetSpecificValue(a*M.Getspecificvalue(1,2)+b*M.Getspecificvalue(2,2),2);
return vec2;
}
double Vector1_2::operator * (const Vector2_1& vec) const
{
return a*vec.GetSpecificValue(1)+b*vec.GetSpecificValue(2);
}
void Vector1_2::print()
{cout<<endl<<"["<<a<<" "<<b<<"]"<<endl;}
Vector2_1 Vector1_2::transpose() const
{
return Vector2_1(a,b);
}
/////////////////MATRIX2_2//////////////////
Matrix2_2::Matrix2_2()
{
M11=0; M12=0; M21=0; M22=0;
}
Matrix2_2::Matrix2_2(const Matrix2_2& M2)
{
*this=M2;
}
void Matrix2_2::SetAllValues(double UPLEFT, double UPRIGHT, double DOWNLEFT, double DOWNRIGHT)
{
M11=UPLEFT; M12=UPRIGHT; M21=DOWNLEFT; M22=DOWNRIGHT;
}
void Matrix2_2::SetSpecificValue(double value, int row, int column)
{
if(row==1)
if(column==1)
M11=value;
else if(column==2)
M12=value;
else
MessageBox(NULL, "wrong way of dealing with Matrix2_2 class", "Located in: void Matrix2_2::SetSpecificValue", NULL);
else if(row==2)
if(column==1)
M21=value;
else if(column==2)
M22=value;
else
MessageBox(NULL, "wrong way of dealing with Matrix2_2 class", "Located in: void Matrix2_2::SetSpecificValue", NULL);
else
MessageBox(NULL, "wrong way of dealing with Matrix2_2 class", "Located in: void Matrix2_2::SetSpecificValue", NULL);
}
double Matrix2_2::Getspecificvalue(int row, int column) const
{
if(row==1)
if(column==1)
return M11;
else if(column==2)
return M12;
else
MessageBox(NULL, "wrong way of dealing with Matrix2_2 class", "Located in: void Matrix2_2::SetSpecificValue", NULL);
else if(row==2)
if(column==1)
return M21;
else if(column==2)
return M22;
else
MessageBox(NULL, "wrong way of dealing with Matrix2_2 class", "Located in: void Matrix2_2::SetSpecificValue", NULL);
else
MessageBox(NULL, "wrong way of dealing with Matrix2_2 class", "Located in: void Matrix2_2::SetSpecificValue", NULL);
return -999999;//for error
}
Matrix2_2 Matrix2_2::operator + (const Matrix2_2& M2) const
{
Matrix2_2 M3;
M3.SetSpecificValue(M11+M2.Getspecificvalue(1,1),1,1);
M3.SetSpecificValue(M12+M2.Getspecificvalue(1,2),1,2);
M3.SetSpecificValue(M21+M2.Getspecificvalue(2,1),2,1);
M3.SetSpecificValue(M22+M2.Getspecificvalue(2,2),2,2);
return M3;
}
Matrix2_2 Matrix2_2::operator - (const Matrix2_2& M2) const
{
Matrix2_2 M3;
M3.SetSpecificValue(M11-M2.Getspecificvalue(1,1),1,1);
M3.SetSpecificValue(M12-M2.Getspecificvalue(1,2),1,2);
M3.SetSpecificValue(M21-M2.Getspecificvalue(2,1),2,1);
M3.SetSpecificValue(M22-M2.Getspecificvalue(2,2),2,2);
return M3;
}
Matrix2_2 Matrix2_2::operator * (const Matrix2_2& M2) const
{
Matrix2_2 mat;
mat.SetSpecificValue(M11*M2.Getspecificvalue(1,1)+M12*M2.Getspecificvalue(2,1), 1,1);
mat.SetSpecificValue(M11*M2.Getspecificvalue(1,2)+M12*M2.Getspecificvalue(2,2), 1,2);
mat.SetSpecificValue(M21*M2.Getspecificvalue(1,1)+M22*M2.Getspecificvalue(2,1), 2,1);
mat.SetSpecificValue(M21*M2.Getspecificvalue(1,2)+M22*M2.Getspecificvalue(2,2), 2,2);
return mat;
}
Vector2_1 Matrix2_2::operator * (const Vector2_1& vec) const
{
Vector2_1 vec2;
vec2.SetSpecificValue(M11*vec.GetSpecificValue(1)+M12*vec.GetSpecificValue(2), 1);
vec2.SetSpecificValue(M21*vec.GetSpecificValue(1)+M22*vec.GetSpecificValue(2), 2);
return vec2;
}
void Matrix2_2::print()
{
cout<<endl<<M11<<" "<<M12<<endl<<M21<<" "<<M22<<endl<<endl;
}
Matrix2_2 Matrix2_2::Transpose() const
{
return Matrix2_2(M11,M21,M12,M22);
}
Kalman filter implementation using these classes
Code:
#include "Matrix.h"
#include <iostream>
using namespace std;
void main()
{
double vel[8]={1,2,3,4,5,6,7,8};
for (int counter=0; counter<8; counter++)
{
static Vector2_1 Xhat_previous(2,2);
const Matrix2_2 A(1,1,0,1);
Vector2_1 XhatPre=A*Xhat_previous;
//XhatPre.print();
static Matrix2_2 P_previous(1,2,3,4);
const Matrix2_2 Q(5,6,7,8);
const double R=0.1666666;
Matrix2_2 Ppre=A*P_previous*A.Transpose()+Q;
//Ppre.print();
const Vector1_2 H(0,1);
Vector2_1 K = (Ppre*H.transpose())/((H*Ppre*H.transpose())+R);
//K.print();
Vector2_1 Xhat=XhatPre+K*(vel[counter]-H*XhatPre);
Xhat.print();
Xhat = Xhat_previous;
const Matrix2_2 I(1,0,0,1);
Matrix2_2 P=(I-K*H)*Ppre;
P.print();
P = P_previous;
}
//return Xhat.GetSpecificValue(1); //this is the calculated velocity from kalman filter. Hhat[2] is the acceleration
}