// Source code for Kalman Filter to convert 2D points to 3D points

/*
Author: Srikanta Patnaik and Team 
Machine Intelligence Laboratory, University College of Engineering, Burla, India
Date of Writing: April, 2003; Version: 1.0
Book: Robot Cognition and Navigation: An Experiment with Mobile Robots
Publisher: Springer
ISBN: 978-3-540-23446-3
URL: http://www.springer.com/3-540-23446-2
*/


(a) Kalman.jav	

//Kalman.java
import java.util.*;
import java.io.*;

class Kalman
{
double u1,v1,f = 12.678;
double x0,y0,z0,A,B,C;
Matrix a0,a1,T,S0,S1,M1,W1,K1,y1;
static String datastring = null;
public Kalman()
{
double mat[][] = {{-0.135,0.049,0,0.09},{0.019,0.053,-0.13,-2.16},{-0.002,-0.008,1.0,1.0}};
double err[][] = {{9999,0,0},{0,9999,0},{0,0,9999}};
	T = new Matrix(3,4);//,mat);
	S0 = new Matrix(3,3,err);
	S1 = new Matrix(3,3);
	a0 = new Matrix(3,1);
	a1 = new Matrix(3,1);

/*	double v1 = 11.099537127043996,
	v2=14.577698983921897,
	v3=30.54117446926405;
	a0.mat[0][0] = v1;// 10.475363831592754;//-10.3;
	a0.mat[1][0] = v2; //13.764154969522421;//0.655;
	a0.mat[2][0] = v3; //28.063386009591593;//17.838;-*/
//	T.show();
//	S0.show();
//	a0.show();
}

public void setValues()
{
a0 = a1;
StringTokenizer st = new StringTokenizer(datastring," ");
u1 = Double.parseDouble(st.nextToken());//-4.4;// -2.7;
v1 = Double.parseDouble(st.nextToken());//1.7;//1.3;
x0 = Double.parseDouble(st.nextToken());//24.0;//36.5;
y0 = Double.parseDouble(st.nextToken());//13.5;//98.5;
z0 = Double.parseDouble(st.nextToken());//28.5;
A = Double.parseDouble(st.nextToken());//0.0;//2.793;
B = Double.parseDouble(st.nextToken());//-1.974;
C = Double.parseDouble(st.nextToken());//0;
}
public Matrix getTi()
{
Matrix Ti = new Matrix(3,4);
Matrix one  = new Matrix(3,4);
one.mat[0][0] = 1;
one.mat[1][1] = 1;
one.mat[2][2] = -1/f;
one.mat[2][3] = 1;

Matrix two = new Matrix(4,4);
two.mat[0][0] = 1;
two.mat[1][1] = 1;
two.mat[2][2] = 1;
two.mat[3][3] = 1;
two.mat[2][3] = f;

Matrix three = new Matrix(4,4);
three.mat[0][0] = 1;
three.mat[1][1] = Math.cos(A);
three.mat[1][2] = -Math.sin(A);
three.mat[2][1] = Math.sin(A);
three.mat[2][2] = Math.cos(A);
three.mat[3][3] = 1;

Matrix four = new Matrix(4,4);
four.mat[1][1] = 1;
four.mat[0][0] = Math.cos(B);
four.mat[0][2] = Math.sin(B);
four.mat[2][0] = -Math.sin(B);
four.mat[2][2] = Math.cos(B);
four.mat[3][3] = 1;

Matrix five = new Matrix(4,4);
five.mat[2][2] = 1;
five.mat[1][1] = Math.cos(C);
five.mat[1][0] = Math.sin(C);
five.mat[0][1] = -Math.sin(C);
five.mat[0][0] = Math.cos(C);
five.mat[3][3] = 1;

Matrix six = new Matrix(4,4);
six.mat[0][0] = 1;
six.mat[1][1] = 1;
six.mat[2][2] = 1;
six.mat[3][3] = 1;
six.mat[0][3] = -x0;
six.mat[1][3] = -y0;
six.mat[2][3] = -z0;

try
{
Ti = Matrix.mulM(one,Matrix.mulM(two,Matrix.mulM three,Matrix.mulM(four,Matrix.mulM(five,six)))));
}
catch(Exception e){}
return Ti;
}

public Matrix getMi()
{
Matrix Mi = new Matrix(2,3);
for(int i=0;i<3;i++)
{
Mi.mat[0][i] = -(u1*T.mat[2][i] - T.mat[0][i]);
Mi.mat[1][i] = -(v1*T.mat[2][i] - T.mat[1][i]);
}
return Mi;
}

public Matrix getWi()
{
Matrix Wi = new Matrix(2,2);
double val=0;
for(int i=0;i<3;i++)
val += T.mat[2][i]*a0.mat[i][0];
Wi.mat[0][0] =Wi.mat[1][1]= -(val - T.mat[2][3]);
return Wi;
}

public Matrix getGain()
{
Matrix Ki = new Matrix(3,2);
M1 = getMi();
W1 = getWi();
try
{
Matrix temp = Matrix.mulM(S0,Matrix.transpose(M1));
Matrix temp1 = Matrix.mulM(M1,temp);
Matrix temp2 = Matrix.sum(W1,temp1);
Ki = Matrix.mulM(temp,Matrix.inverse2(temp2));
}
catch(Exception e){}
return Ki;
}

public Matrix getYi()
{
		
T = getTi();
T.show();
Matrix y = new Matrix(2,1);

y.mat[0][0] = T.mat[2][3]*u1 - T.mat[0][3];
y.mat[1][0] = T.mat[2][3]*v1 - T.mat[1][3];

return y;
}
public Matrix getState()
{
K1 = getGain();
y1 = getYi();
		
try
{
Matrix temp = Matrix.mulM(M1,a0);
temp.mulK(-1);
Matrix temp1 = Matrix.sum(y1,temp);
Matrix temp2 = Matrix.mulM(K1,temp1);
a1 = Matrix.sum(a0,temp2);
}
catch(Exception e){}

return a1;
}

public Matrix getCov()
{
Matrix I = new Matrix(3,3);
I.identity();
try
{
Matrix temp = Matrix.mulM(K1,M1);
temp.mulK(-1);
Matrix temp2 = Matrix.sum(I,temp);
S1 = Matrix.mulM(temp2,S0); 
}catch(Exception e){}
return S1;
}

public static void main(String[] args) 
{
System.out.println("Hello World!");
Kalman k = new Kalman();
		
try
{
FileInputStream fin = new FileInputStream("input.dat");
DataInputStream din = new DataInputStream(fin);
FileOutputStream fout = new FileOutputStream("output.dat");
DataOutputStream dout = new DataOutputStream(fout);

for(int i=0;i<6;)
{
datastring = din.readLine();i++;
	k.setValues();
Matrix m = k.getYi();//m.show();
m = Matrix.transpose(k.getState());m.show(dout);
System.out.println("_______________________________");
			}
	
//	m = k.getCov();m.show();
//	m = k.getMi();m.show();
}catch(Exception e){}
}
}

                               
(b) Matrix.jav

//Matrix.jav
import java.util.*;
import java.io.*;

public class Matrix
{
double mat[][];
int m,n;
public Matrix(int mm,int nn)
{
m=mm;
n=nn;
mat = new double[m][n];
for(int i=0;i<m;i++)
for(int j=0;j<n;j++)
mat[i][j]=0;
}
public Matrix(int mm,int nn,double matrix[][])
{
m=mm;
n=nn;
mat = new double[m][n];
for(int i=0;i<m;i++)
for(int j=0;j<n;j++)
mat[i][j]=matrix[i][j];
}
public void identity()
{
for(int i =0;i<m;i++)
for(int j=0;j<n;j++)
{
if(i==j)
mat[i][j]=1;
}
}
public Matrix mulK(double konst)
{
for(int i=0;i<m;i++)
for(int j=0;j<n;j++)
mat[i][j]*=konst;
return this;
}
public static Matrix mulM(Matrix a,Matrix b) throws Exception
{
Matrix product;
if(a.n!=b.m)
throw(new Exception());
else
{
product = new Matrix(a.m,b.n);
for(int i=0;i<a.m;i++)
{
for(int j=0;j<b.n;j++)
{
for(int k=0;k<a.n;k++)
{
product.mat[i][j] += (a.mat[i][k])*(b.mat[k][j]);
}
}
}
}
return product;	
}
public static Matrix transpose(Matrix A)
{
Matrix AT = new Matrix(A.n,A.m);
for(int i=0;i<A.m;i++)
	for(int j=0;j<A.n;j++)
AT.mat[j][i] = A.mat[i][j];
return AT;
}
public static Matrix sum(Matrix A,Matrix B)throws Exception
{
Matrix ab;
if((A.m!=B.m)||(A.n!=B.n))
throw(new Exception());
			
else
{
ab = new Matrix(A.m,A.n);
for(int i=0;i<A.m;i++)
{
for(int j=0;j<A.n;j++)
{
ab.mat[i][j] = A.mat[i][j]+B.mat[i][j];
}
}
}
return ab;
}
public static double determinant(Matrix A)
{
return 0;
}

public static Matrix inverse2(Matrix A)
{
double det = A.mat[0][0]*A.mat[1][1] - A.mat[0][1]*A.mat[1][0];
double temp = A.mat[0][0]/det;
A.mat[0][0] = A.mat[1][1]/det;
A.mat[1][1] = temp;
A.mat[0][1] = -A.mat[0][1]/det;
A.mat[1][0] = -A.mat[1][0]/det;
return A;
}

public void show()
{
int i,j;
for (i=0;i<m;i++)
{
for (j=0;j<n-1;j++)
{
System.out.print(mat[i][j]+" ");
}
System.out.println(mat[i][j]);
}
}
	
public void show(DataOutputStream dout)
{
int i,j;
try
{
for (i=0;i<m;i++)
{
for (j=0;j<n;j++)
{
dout.writeBytes(new String(String.valueOf(mat[i][j])+" "));
}
dout.writeByte('\n');
}
}catch(Exception e){}
}

public void init()
{
byte buf[]=new byte[50];
for(int i=0;i<m;i++)
{
for(int j=0;j<n;j++)
{
try
{
System.in.read(buf);
String st = new String(buf);
mat[i][j] = Double.parseDouble(st);
}catch(Exception e){}
}
}
}

public Vector get3Vector(int m)
{
Vector v = new Vector(3);
for(int i = 0;i<3;i++)
v.add(new Double(mat[m][i]));
return v;
}
		
public static void main(String[] args) 
{
System.out.println("Hello World!");
Matrix A = new Matrix(3,3);
A.init();
try
{
A=Matrix.mulM(A,A);
}catch(Exception e){}
A.show();
}
}                                   
