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
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
|
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "kalman_filter.h"
/*
* Performs alpha * A * B + beta * C
*/
extern void sgemm_(
char *TRANSA, // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
char *TRANSB, // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
int *M, // INPUT: Size parameter 'M'
int *N, // INPUT: Size parameter 'N'
int *K, // INPUT: Size parameter 'K'
float *ALPHA, // INPUT: scaling coefficient for A * B
void *A, // INPUT: (float) Column array 'A' (M by K)
int *LDA, // INPUT: Column stride for 'A'
void *B, // INPUT: (float) Column array 'B' (K by N)
int *LDB, // INPUT: Column stride for 'B'
float *BETA, // INPUT: Scaling factor for 'C'
void *C, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
int *LDC); // INPUT: Column stride for 'C'
/*
* General A X = B solution
*/
extern void sgesv_(
int *N, // INPUT: The order of matrix A
int *NRHS, // INPUT: the number of columns of matrix B
void *A, // INPUT/OUTPUT: (float) Column array
// entry: (N by N) matrix A
// exit: (L and U) from factorization A = P*L*U
int *LDA, // INPUT: Column stride of A
int *IPIV, // OUTPUT: Ineger array (dimension N) the pivot indices of the permutation matrix
int *B, // INPUT/OUTPUT: (float) Column array
// entry: (N by NRHS) matrix of right hand side matrix 'B'
// exit: (if INFO=0) N-NRHS solution matrix 'X'
int *LDB, // INPUT: Column stride of B
int *INFO); // OUTPUT: Did it work?
// 0: success
// < 0: if INFO==-i, the -ith argument had illegal val
// > 0: if INFO== i, U(i,i) is exactly zero, thus the factorization is singular (error).
void KalmanPredict(
KAL_VEC(xhat_k_km1), /* OUTPUT: (S) Predicted state at time 'k' */
KAL_MAT(P_k_km1), /* OUTPUT: (S x S) Predicted covariance at time 'k' */
KAL_MAT(P_km1_km1), /* INPUT: (S x S) Updated covariance from time 'k-1' */
KAL_VEC(xhat_km1_km1), /* INPUT: (S) Updated state from time 'k-1' */
KAL_MAT(F_k), /* INPUT: (S x S) State transition model */
KAL_MAT(B_k), /* INPUT: (S x U) Control input model */
KAL_VEC(u_k), /* INPUT: (U) Control vector */
KAL_MAT(Q_k), /* INPUT: (S x S) Covariance of process noise */
int S, /* INPUT: Number of dimensions in state vector */
int U) /* INPUT: Size of control input vector */
{
/* Temporary storage */
KAL_MAT(F_k__times__P_km1_km1);
#define B_k__times__u_k xhat_k_km1 /* This vector overwrite the same memory */
/* Fortran pass by pointer */
float zerof=0.0f;
float onef=1.0f;
int one=1;
int stride=KAL_STRIDE;
int x,y;
/* Calculate B_k__times__u_k */
if (U == 0) {
/* If no control input model, then zero out */
memset(B_k__times__u_k, 0, S*sizeof(float));
} else {
/* Otherwise: B_k * u_k */
sgemm_(
"n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
"n", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
&S, // INPUT: Size parameter 'M'
&one, // INPUT: Size parameter 'N'
&U, // INPUT: Size parameter 'K'
&onef, // INPUT: scaling coefficient for A * B
B_k, // INPUT: (float) Column array 'A' (M by K)
&stride, // INPUT: Column stride for 'A'
u_k, // INPUT: (float) Column array 'B' (K by N)
&stride, // INPUT: Column stride for 'B'
&zerof, // INPUT: Scaling factor for 'C'
B_k__times__u_k, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
&stride); // INPUT: Column stride for 'C'
}
/* Calculate xhat_k_km1 */
sgemm_(
"n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
"n", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
&S, // INPUT: Size parameter 'M'
&one, // INPUT: Size parameter 'N'
&S, // INPUT: Size parameter 'K'
&onef, // INPUT: scaling coefficient for A * B
F_k, // INPUT: (float) Column array 'A' (M by K)
&stride, // INPUT: Column stride for 'A'
xhat_km1_km1, // INPUT: (float) Column array 'B' (K by N)
&stride, // INPUT: Column stride for 'B'
&onef, // INPUT: Scaling factor for 'C'
xhat_k_km1, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
&stride); // INPUT: Column stride for 'C'
/* Calculate F_k * P_km1_km1*/
sgemm_(
"n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
"n", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
&S, // INPUT: Size parameter 'M'
&S, // INPUT: Size parameter 'N'
&S, // INPUT: Size parameter 'K'
&onef, // INPUT: scaling coefficient for A * B
F_k, // INPUT: (float) Column array 'A' (M by K)
&stride, // INPUT: Column stride for 'A'
P_km1_km1, // INPUT: (float) Column array 'B' (K by N)
&stride, // INPUT: Column stride for 'B'
&zerof, // INPUT: Scaling factor for 'C'
F_k__times__P_km1_km1, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
&stride); // INPUT: Column stride for 'C'
/* Calculate P_k_km1 */
for (x=0; x<S; x++) {
for (y=0; y<S; y++) {
P_k_km1[x][y] = Q_k[x][y];
}
}
sgemm_(
"n", // INPUT: Do we transpose A? 'n' no transpose, 't' transpose
"t", // INPUT: Do we transpose B? 'n' no transpose, 't' transpose
&S, // INPUT: Size parameter 'M'
&S, // INPUT: Size parameter 'N'
&S, // INPUT: Size parameter 'K'
&onef, // INPUT: scaling coefficient for A * B
F_k__times__P_km1_km1, // INPUT: (float) Column array 'A' (M by K)
&stride, // INPUT: Column stride for 'A'
F_k, // INPUT: (float) Column array 'B' (K by N)
&stride, // INPUT: Column stride for 'B'
&zerof, // INPUT: Scaling factor for 'C'
P_k_km1, // INPUT/OUTPUT: (float) Column array 'C' RESULT PLACED HERE
&stride); // INPUT: Column stride for 'C'
}
void KalmanUpdate(
KAL_VEC(xhat_k_k), /* OUTPUT: Updated state at time 'k' */
KAL_MAT(P_k_k), /* OUTPUT: Updated covariance at time 'k' */
KAL_VEC(xhat_k_km1), /* INPUT: Predicted state at time 'k' */
KAL_MAT(P_k_km1), /* INPUT: Predicted covariance at time 'k' */
KAL_MAT(H_k), /* INPUT: Observational model */
KAL_MAT(R_k), /* INPUT: Covariance of observational noise */
int B, /* INPUT: Number of observations in observation vector */
int S) /* INPUT: Number of measurements in the state vector */
{
}
|
