1 files changed, 118 insertions, 0 deletions
diff --git a/src/c/signalProcessing/kalm/dkalma.c b/src/c/signalProcessing/kalm/dkalma.c
new file mode 100644
index 0000000..d71a86a
--- /dev/null
+++ b/src/c/signalProcessing/kalm/dkalma.c
@@ -0,0 +1,118 @@
+/* Copyright (C) 2017 - IIT Bombay - FOSSEE
+
+ This file must be used under the terms of the CeCILL.
+ This source file is licensed as described in the file COPYING, which
+ you should have received as part of this distribution.  The terms
+ are also available at
+ http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt
+ Author: Abhinav Dronamraju
+ Organization: FOSSEE, IIT Bombay
+ Email: toolbox@scilab.in
+ */
+
+/*Function to find kalm */
+
+#include "lapack.h"
+#include "stdlib.h"
+#include "stdio.h"
+#include "string.h"
+#include "kalm.h"
+#include "matrixTranspose.h"
+#include "matrixMultiplication.h"
+#include "matrixInversion.h"
+#include "addition.h"
+#include "subtraction.h"
+#include "eye.h"
+
+void dkalma(double* y, int y_row, int y_col, double* x0, int x0_row, int x0_col, double* p0, int p0_row, int p0_col, double* f, int f_row, int f_col, double* g, int g_row, int g_col, double* h, int h_row, int h_col, double* q, int q_row, int q_col, double* r, int r_row, int r_col, double* x1, double* p1, double* x, double* p)
+	    
+{ 
+ double k[p0_row*h_row];
+ double h_trans[h_col*h_row];
+ double temp1[p0_row*h_row];
+ double temp2[h_row*p0_col];
+ double temp3[h_row*h_row];
+ double temp4[h_row*h_row];
+ double temp5[h_row*h_row];
+ double temp6[p0_row*p0_col];
+ double temp7[p0_row*h_col];
+ double temp8[p0_row*p0_col];
+ double f_trans[f_col*f_row];
+ double g_trans[g_col*g_row];
+ double temp9 [f_row*p0_col];
+ double temp10[f_row*f_row];
+ double temp11 [g_row*q_col];
+ double temp12[g_row*g_row];
+ double temp13[h_row*x0_col];
+ double temp14[h_row*x0_col];
+ double temp15[p0_row*x0_col];
+
+
+
+ dtransposea(h, h_row, h_col, h_trans);
+ dmulma(p0, p0_row, p0_col, h_trans, h_col, h_row, temp1);    //temp1= p0*h'
+ dmulma(h, h_row, h_col, p0, p0_row, p0_col, temp2);          //temp2= h*p0
+ dmulma(temp2, h_row, p0_col, h_trans, h_col, h_row, temp3);  //temp3= h*p0*h'
+ dadda(temp3, h_row*h_row, r, r_row*r_col, temp4);            //temp4= h*p0*h'+r
+
+/*//Debug Only*/
+/* for(int i=0; i< h_row*h_row; i++)*/
+/*	printf("temp4[%d] = %lf \t",i, temp4[i]);*/
+/*   printf("\n");*/
+
+ dinverma(temp4, temp5, h_row);                               //temp5= (h*p0*h'+r)**(-1)
+
+ dmulma(temp1, p0_row, h_row, temp5, h_row, h_row, k);        //k=p0*h'*(h*p0*h'+r)**(-1)
+
+/*//Debug Only*/
+/* for(int i=0; i< p0_row*h_row; i++)*/
+/*	printf("k[%d] = %lf \t",i, k[i]);*/
+/*   printf("\n");*/
+   
+   
+ deyea(temp6, p0_row, p0_col);                                //temp6 = eye(p0)
+ dmulma(k, p0_row, h_row, h, h_row, h_col, temp7);            //temp7 = k*h
+ ddiffa(temp6, p0_row*p0_col, temp7, p0_row*h_col, temp8);    //temp8= eye(p0)- k*h
+ dmulma(temp8, p0_row, p0_col, p0, p0_row, p0_col, p);        //p=(eye(p0)-k*h)*p0
+
+/*//Debug Only*/
+/* for(int i=0; i< p0_row*p0_col; i++)*/
+/*	printf("p[%d] = %lf \t",i, p[i]);*/
+/*   printf("\n");*/
+
+
+ dtransposea(g, g_row, g_col, g_trans);
+ dtransposea(f, f_row, f_col, f_trans);
+ dmulma(f, f_row, f_col, p, p0_row, p0_col, temp9);             //temp9= f*p
+ dmulma(temp9, f_row, p0_col, f_trans, f_col, f_row, temp10);  //temp10= f*p*f'
+ dmulma(g, g_row, g_col, q, q_row, q_col, temp11);            //temp11= g*q
+ dmulma(temp11, g_row, q_col, g_trans, g_col, g_row, temp12); //temp12= g*q*g'
+ dadda(temp10, f_row*f_row, temp12, g_row*g_row, p1);         //p1=f*p*f'+g*q*g'
+
+/* //Debug Only*/
+/* for(int i=0; i< f_row*f_row; i++)*/
+/*	printf("p1[%d] = %lf \t",i, p1[i]);*/
+/*   printf("\n");*/
+ 
+ 
+ dmulma(h, h_row, h_col, x0, x0_row, x0_col, temp13);         //temp13= h*x0
+ ddiffa(y, y_row*y_col, temp13, h_row*x0_col, temp14);        //temp14= y-h*x0
+ dmulma(k, p0_row, h_row, temp14, h_row, x0_col, temp15);   //temp15= k*(y-h*x0)
+ dadda(x0, x0_row*x0_col, temp15, p0_row*x0_col, x);          //x=x0+k*(y-h*x0) 
+
+/* //Debug Only*/
+/* for(int i=0; i< x0_row*x0_col; i++)*/
+/*	printf("x[%d] = %lf \t",i, x[i]);*/
+/*   printf("\n");*/
+ 
+
+ dmulma(f, f_row, f_col, x, x0_row, x0_col, x1) ;              //x1= f*x
+
+/* //Debug Only*/
+/* for(int i=0; i< f_row*x0_col; i++)*/
+/*	printf("x1[%d] = %lf \t",i, x1[i]);*/
+/*   printf("\n");*/
+ 
+
+
+}