1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
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");*/
}
|
