// Kalman Update Step
// We "update" given what we get for data
// The filter will use the data given to lower our
// uncertainty (aka. covariance)
void KalmanUpdate(struct KalmanFilter *kal, struct Matrix meas)
{
struct Matrix y, S, extTran, K, Sinv, x_next, P_next;
CreateBlankMatrix(y);
CreateBlankMatrix(S);
CreateBlankMatrix(extTran);
CreateBlankMatrix(K);
CreateBlankMatrix(Sinv);
CreateBlankMatrix(x_next);
CreateBlankMatrix(P_next);
CopyMatrix(&kal->measurementVector, meas);
// Find the difference between the move (measurement)
// and what we predicted (extraction * data)
MatrixMult(&y, kal->extractionMatrix, kal->meanVector);
MatrixSub(&y, kal->measurementVector, y);
// The Covariance of the move
MatrixMult(&S, kal->extractionMatrix, kal->covarianceMatrixX);
MatrixTranspose(&extTran, kal->extractionMatrix);
MatrixMult(&S, S, extTran);
MatrixAdd(&S, S, kal->covarianceMatrixZ);
// Kalman Gain
MatrixInv(&Sinv, S);
MatrixMult(&K, kal->covarianceMatrixX, extTran);
MatrixMult(&K, K, Sinv);
// Figure out mean and covariance results
MatrixMult(&x_next, K, y);
MatrixAdd(&x_next, kal->meanVector, x_next);
MatrixMult(&P_next, kal->covarianceMatrixX, extTran);
MatrixMult(&P_next, P_next, Sinv);
MatrixMult(&P_next, P_next, kal->extractionMatrix);
MatrixMult(&P_next, P_next, kal->covarianceMatrixX);
MatrixSub(&P_next, kal->covarianceMatrixX, P_next);
// Copy results to the kalmanfilter class
CopyMatrixByValue(&kal->meanVector, x_next);
CopyMatrixByValue(&kal->covarianceMatrixX, P_next);
// Delete matricies so we don't have memory leaks..
DeleteMatrix(&y);
DeleteMatrix(&S);
DeleteMatrix(&extTran);
DeleteMatrix(&K);
DeleteMatrix(&Sinv);
DeleteMatrix(&x_next);
DeleteMatrix(&P_next);
}
int main(int argc, char ** argv)
{
char op;
char mStr[1024];
matrix ans, m1, m2;
int sc = 0;
if(argc > 1){
op = getOp(argv[1]);
} else {
printf("Which operation: ");
op = getOp(NULL);
}
printf("First matrix:\n");
scanf("%s", mStr);
m1 = MatrixInit(mStr);
if(op == 'a' || op == 's' || op == 'm'){
printf("Second matrix:\n");
scanf("%s", mStr);
m2 = MatrixInit(mStr);
} else if(op == 'c') {
printf("Scalar multiple:\n");
scanf("%d", &sc);
}
switch(op){
case 'a':
ans = MatrixAdd(m1, m2);
break;
case 's':
ans = MatrixSub(m1, m2);
break;
case 'm':
ans = MatrixMul(m1, m2);
break;
case 'i':
ans = MatrixInv(m1);
break;
case 'c':
ans = MatrixSMul(m1, i2f(sc));
break;
default:
printf("Something went very wrong.\n");
return 1;
}
printf("Answer:\n");
MatrixPrint(ans);
MatrixFree(m1);
MatrixFree(ans);
if(op == 'a' || op == 's' || op == 'm'){
MatrixFree(m2);
}
return 0;
}
task main()
{
// Initiate BNS Library
BNS();
// Create a 3x3 matrix of zeros
Matrix mat1;
CreateZerosMatrix(&mat1, 3, 3);
// Create a 3x3 matrix with some data in it
// Set location 1 down, 0 across, to be 16
Matrix mat2;
CreateMatrix(&mat2, "1.10 3.40 0; 5 3 2; 0 1 1.234");
SetMatrixAt(&mat2, 1, 0, 16);
// Creates a 3x3 identity matrix, then multiply it by 11
Matrix mat3;
CreateIdentityMatrix(&mat3, 3);
MatrixMultiplyScalar(&mat3, 11);
// Print matricies to the debugger console
PrintMatrix(&mat1);
PrintMatrix(&mat2);
PrintMatrix(&mat3);
// Matrix Examples:
// Matrix determinant
float det = MatrixDeterminant(&mat2);
writeDebugStreamLine("Matrix Det = %f", det);
// Matrix Inverse
Matrix inv;
MatrixInv(&inv, mat2);
writeDebugStream("Inverse ");
PrintMatrix(&inv);
// Matrix Multiplication
Matrix mult;
MatrixMult(&mult, mat2, mat3);
writeDebugStream("Multiply ");
PrintMatrix(mult);
// Matrix Addition
Matrix add;
MatrixAdd(&add, mat2, mat3);
writeDebugStream("Add ");
PrintMatrix(&add);
// Matrix Subtraction
Matrix sub;
MatrixSub(&sub, mat3, mat2);
writeDebugStream("Subtract ");
PrintMatrix(&sub);
}
//-------------------main function starts here------------------------------------
void main()
{
float a[ROW][COL], b[ROW][COL];
int m[2], n[2], choice,i,j,temp;
bool flag, flag1=true;
//-------------from the following the size of matrix is tested with proper input----------------------
cout<<"Enter the dimention of the 1st matrix :"<<endl;
for(i=0;i<2;i++)
{
flag = false;
while(!flag)
{
cin >> temp;
if(temp>0 && !cin.fail() && (cin.peek() == EOF || cin.peek() == '\n'))
{
flag = true;
m[i]=temp;
}
else
{
cout << "Please Enter valid data" << endl;
cin.clear();
cin.ignore(numeric_limits<streamsize>::max(), '\n');
}
}
}
cout<<"Enter the dimention of the 2nd matrix :"<<endl;
for(i=0;i<2;i++)
{
flag = false;
while(!flag)
{
cin >> temp;
if(temp>0 && !cin.fail() && (cin.peek() == EOF || cin.peek() == '\n'))
{
flag = true;
n[i]=temp;
}
else
{
cout << "Please Enter valid data" << endl;
cin.clear();
cin.ignore(numeric_limits<streamsize>::max(), '\n');
}
}
}
//---------------input for 1st matrix starts here----------------------
cout<<"Enter the 1st matrix"<<endl;
for(i=0;i<m[0];i++)
{
for(j=0;j<m[1];j++)
cin>>a[i][j];
}
//--------------input for 2nd matrix strts here-------------------------
cout<<"Enter the 2nd matrix"<<endl;
for(i=0;i<n[0];i++)
{
for(j=0;j<n[1];j++)
cin>>b[i][j];
}
//-------------Choice for Matrix operation to select------------------
while(1)
{
if(!flag1)
{
exit(0);
}
cout<<"Select the operation to perform with the above matrix \n 1. Addition\n 2. Subtraction\n 3. Multiplication"<<endl;
cin>>choice;
switch(choice)
{
case 1:
if(m[0]!=n[0] || m[1]!=n[1])
cout<<"Matrix cannot be added"<<endl;
else
MatrixAdd(a,b,m);
break;
case 2:
if(m[0]!=n[0] || m[1]!=n[1])
cout<<"Matrix cannot be subtracted"<<endl;
else
MatrixSub(a,b,m);
break;
case 3:
if(m[1]!=n[0])
cout<<"Matrix cannot be multiplied";
else
MatrixMul(a,b,m,n);
break;
default:
cout<<"Invalid Input!!"<<endl;
}
//-----------------Asked for continuity or to exit-------------------------------
cout<<"Enter 1 to try again or else 0 to exit : ";
cin>>flag1;
}
}
int _tmain(int argc, _TCHAR* argv[])
{
bool result = false;
TArgInfo info;
ParseArg(info,argc,argv);
const std::string &m = info.m_mode;
//printf("mode=%s\n", m.c_str());
if("f32_hex" == m){
result = Float32ToHex(info);
}else if("f64_hex" == m){
result = Float64ToHex(info);
}else if("hex_f32" == m){
result = HexToFloat32(info);
}else if("hex_f64" == m){
result = HexToFloat64(info);
}else if("2_10" == m){
result = ConvertNumber(info,10,2);
}else if("2_16" == m){
result = ConvertNumber(info,16,2);
}else if("10_2" == m){
result = ConvertNumber(info,2,10);
}else if("10_16" == m){
result = ConvertNumber(info,16,10);
}else if("16_2" == m){
result = ConvertNumber(info,2,16);
}else if("16_10" == m){
result = ConvertNumber(info,10,16);
}else if("r_d" == m){
result = RadianToDegree(info);
}else if("d_r" == m){
result = DegreeToRadian(info);
}else if("v_h" == m){
result = MatrixSub(info,FovVerticalToHorizontal,3);
}else if("h_v" == m){
result = MatrixSub(info,FovHorizontalToVertical,3);
}else if("ql" == m){
result = MatrixSub(info,QuatLength,4);
}else if("qn" == m){
result = MatrixSub(info,QuatNormalize,4);
}else if("qi" == m){
result = MatrixSub(info,QuatInverse,4);
}else if("q_m33" == m){
result = MatrixSub(info,QuatToMat,4);
}else if("q_a" == m){
result = MatrixSub(info,QuatToAAng,4);
}else if("q_xyz" == m){
result = MatrixSub(info,QuatToXYZ,4);
}else if("m33_q" == m){
result = MatrixSub(info,Mat33ToQuat,9);
}else if("m33_a" == m){
result = MatrixSub(info,Mat33ToAAng,9);
}else if("m33t" == m){
result = MatrixSub(info,Mat33Transpose,9);
}else if("m33d" == m){
result = MatrixSub(info,Mat33Determinant,9);
}else if("m33i" == m){
result = MatrixSub(info,Mat33Invert,9);
}else if("m33_s" == m){
result = MatrixSub(info,Mat33Scale,9);
}else if("x_m33" == m){
result = MatrixSub(info,Mat33RotX,1);
}else if("y_m33" == m){
result = MatrixSub(info,Mat33RotY,1);
}else if("z_m33" == m){
result = MatrixSub(info,Mat33RotZ,1);
}else if("yxz_m33" == m){
result = MatrixSub(info,Mat33RotYXZ,3);
}else if("zxy_m33" == m){
result = MatrixSub(info,Mat33RotZXY,3);
}else if("zyx_m33" == m){
result = MatrixSub(info,Mat33RotZYX,3);
}else if("yzx_m33" == m){
result = MatrixSub(info,Mat33RotYZX,3);
}else if("xzy_m33" == m){
result = MatrixSub(info,Mat33RotXZY,3);
}else if("xyz_m33" == m){
result = MatrixSub(info,Mat33RotXYZ,3);
}else if("m33_yxz" == m){
result = MatrixSub(info,Mat33EulerYXZ,9);
}else if("m33_zxy" == m){
result = MatrixSub(info,Mat33EulerZXY,9);
}else if("m33_zyx" == m){
result = MatrixSub(info,Mat33EulerZYX,9);
}else if("m33_yzx" == m){
result = MatrixSub(info,Mat33EulerYZX,9);
}else if("m33_xzy" == m){
result = MatrixSub(info,Mat33EulerXZY,9);
}else if("m33_xyz" == m){
result = MatrixSub(info,Mat33EulerXYZ,9);
}else if("v3l" == m){
result = MatrixSub(info,Vector3Length,3);
}else if("v3n" == m){
result = MatrixSub(info,Vector3Normalize,3);
}
if(result){
return 0;
}
usage();
return 1;
}
int main(void)
{
char mStr[1024];
matrix *ans, *m1, *m2;
int sc = 0;
printf("Which operation: ");
char op = tolower(getchar());
while(op != '+' || op != '-' || op != '*' || op != '/' || op != 'i') {
puts(opErr);
op = tolower(getchar());
}
printf("First matrix:\n");
scanf("%s", mStr);
m1 = MatrixInit(mStr);
MatrixPrint(m1);
if(op == 'a' || op == 's' || op == 'm') {
printf("Second matrix:\n");
scanf("%s", mStr);
m2 = MatrixInit(mStr);
MatrixPrint(m2);
} else if(op == 'c') {
printf("Scalar multiple:\n");
scanf("%d", &sc);
}
switch(op) {
case 'a':
ans = MatrixAdd(m1, m2);
break;
case 's':
ans = MatrixSub(m1, m2);
break;
case 'm':
ans = MatrixMul(m1, m2);
break;
case 'i':
ans = MatrixInv(m1);
break;
case 'c':
ans = MatrixSMul(m1, sc);
break;
default:
printf("Something went very wrong.\n");
return 1;
}
printf("Answer:\n");
MatrixPrint(ans);
MatrixFree(m1);
MatrixFree(ans);
if(op == 'a' || op == 's' || op == 'm') {
MatrixFree(m2);
}
return 0;
}
