Erick Rodriguez / Mbed OS USNA-UMBC-KF-02_v3-noise

USNA-UMBC-Project Receiver - Add noise to CAN-bus received data and Implement Kalman Filter

Dependencies:   ServoOut mcp2515 BNO055

gtrackMatrix.c@1:5794ff4efa9a, 23 months ago (annotated)

Committer:
professorrodriguezse
Date:
Fri May 20 18:37:10 2022 +0000
Revision:
1:5794ff4efa9a
USNA-UMBC-KalmanFilter-ReceiverSide

Who changed what in which revision?

UserRevisionLine numberNew contents of line
professorrodriguezse 1:5794ff4efa9a 1 /**
professorrodriguezse 1:5794ff4efa9a 2 * @b Description
professorrodriguezse 1:5794ff4efa9a 3 * @n
professorrodriguezse 1:5794ff4efa9a 4 * This function is used to create identity matrix
professorrodriguezse 1:5794ff4efa9a 5 *
professorrodriguezse 1:5794ff4efa9a 6 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 7 * Size of identity matrix
professorrodriguezse 1:5794ff4efa9a 8 * @param[out] A
professorrodriguezse 1:5794ff4efa9a 9 * Matrix A(size,size) = eye(size)
professorrodriguezse 1:5794ff4efa9a 10 *
professorrodriguezse 1:5794ff4efa9a 11 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 12 *
professorrodriguezse 1:5794ff4efa9a 13 * @retval
professorrodriguezse 1:5794ff4efa9a 14 * None
professorrodriguezse 1:5794ff4efa9a 15 */
professorrodriguezse 1:5794ff4efa9a 16 void gtrack_matrixEye(int size, float *A)
professorrodriguezse 1:5794ff4efa9a 17 {
professorrodriguezse 1:5794ff4efa9a 18 /* A(size*size) = eye(size) */
professorrodriguezse 1:5794ff4efa9a 19 int i;
professorrodriguezse 1:5794ff4efa9a 20
professorrodriguezse 1:5794ff4efa9a 21 for (i = 0U; i < size*size; i++) {
professorrodriguezse 1:5794ff4efa9a 22 A[i] = 0.f;
professorrodriguezse 1:5794ff4efa9a 23 }
professorrodriguezse 1:5794ff4efa9a 24
professorrodriguezse 1:5794ff4efa9a 25 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 26 A[i+i*size] = 1.0f;
professorrodriguezse 1:5794ff4efa9a 27 }
professorrodriguezse 1:5794ff4efa9a 28 }
professorrodriguezse 1:5794ff4efa9a 29
professorrodriguezse 1:5794ff4efa9a 30 /**
professorrodriguezse 1:5794ff4efa9a 31 * @b Description
professorrodriguezse 1:5794ff4efa9a 32 * @n
professorrodriguezse 1:5794ff4efa9a 33 * This function is used to initialise matrix to a value
professorrodriguezse 1:5794ff4efa9a 34 *
professorrodriguezse 1:5794ff4efa9a 35 * @param[in] rows
professorrodriguezse 1:5794ff4efa9a 36 * Number of rows
professorrodriguezse 1:5794ff4efa9a 37 * @param[in] cols
professorrodriguezse 1:5794ff4efa9a 38 * Number of cols
professorrodriguezse 1:5794ff4efa9a 39 * @param[in] value
professorrodriguezse 1:5794ff4efa9a 40 * value to set
professorrodriguezse 1:5794ff4efa9a 41 * @param[out] A
professorrodriguezse 1:5794ff4efa9a 42 * Matrix A(rows,cols) = ones(rows,cols)*value
professorrodriguezse 1:5794ff4efa9a 43 *
professorrodriguezse 1:5794ff4efa9a 44 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 45 *
professorrodriguezse 1:5794ff4efa9a 46 * @retval
professorrodriguezse 1:5794ff4efa9a 47 * None
professorrodriguezse 1:5794ff4efa9a 48 */
professorrodriguezse 1:5794ff4efa9a 49 void gtrack_matrixInit(int rows, int cols, float value, float *A)
professorrodriguezse 1:5794ff4efa9a 50 {
professorrodriguezse 1:5794ff4efa9a 51 /* A(rows*cols) = ones(rows,cols)*value */
professorrodriguezse 1:5794ff4efa9a 52 int i;
professorrodriguezse 1:5794ff4efa9a 53
professorrodriguezse 1:5794ff4efa9a 54 for (i = 0U; i < rows*cols; i++) {
professorrodriguezse 1:5794ff4efa9a 55 A[i] = value;
professorrodriguezse 1:5794ff4efa9a 56 }
professorrodriguezse 1:5794ff4efa9a 57 }
professorrodriguezse 1:5794ff4efa9a 58
professorrodriguezse 1:5794ff4efa9a 59 /**
professorrodriguezse 1:5794ff4efa9a 60 * @b Description
professorrodriguezse 1:5794ff4efa9a 61 * @n
professorrodriguezse 1:5794ff4efa9a 62 * This function is used to create diagonal square matrix
professorrodriguezse 1:5794ff4efa9a 63 *
professorrodriguezse 1:5794ff4efa9a 64 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 65 * Size of square matrix
professorrodriguezse 1:5794ff4efa9a 66 * @param[in] v
professorrodriguezse 1:5794ff4efa9a 67 * Diagonal vector
professorrodriguezse 1:5794ff4efa9a 68 * @param[out] A
professorrodriguezse 1:5794ff4efa9a 69 * Matrix A(size,size) = diag(v(size))
professorrodriguezse 1:5794ff4efa9a 70 *
professorrodriguezse 1:5794ff4efa9a 71 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 72 *
professorrodriguezse 1:5794ff4efa9a 73 * @retval
professorrodriguezse 1:5794ff4efa9a 74 * None
professorrodriguezse 1:5794ff4efa9a 75 */
professorrodriguezse 1:5794ff4efa9a 76 void gtrack_matrixSetDiag(int size, float *v, float *A)
professorrodriguezse 1:5794ff4efa9a 77 {
professorrodriguezse 1:5794ff4efa9a 78 /* A(size*size) = diag(v(size)) */
professorrodriguezse 1:5794ff4efa9a 79 int i;
professorrodriguezse 1:5794ff4efa9a 80
professorrodriguezse 1:5794ff4efa9a 81 for (i = 0U; i < size*size; i++) {
professorrodriguezse 1:5794ff4efa9a 82 A[i] = 0;
professorrodriguezse 1:5794ff4efa9a 83 }
professorrodriguezse 1:5794ff4efa9a 84
professorrodriguezse 1:5794ff4efa9a 85 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 86 A[i+i*size] = v[i];
professorrodriguezse 1:5794ff4efa9a 87 }
professorrodriguezse 1:5794ff4efa9a 88 }
professorrodriguezse 1:5794ff4efa9a 89
professorrodriguezse 1:5794ff4efa9a 90 /**
professorrodriguezse 1:5794ff4efa9a 91 * @b Description
professorrodriguezse 1:5794ff4efa9a 92 * @n
professorrodriguezse 1:5794ff4efa9a 93 * This function is used to get diagonal from the square matrix
professorrodriguezse 1:5794ff4efa9a 94 *
professorrodriguezse 1:5794ff4efa9a 95 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 96 * Size of square matrix
professorrodriguezse 1:5794ff4efa9a 97 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 98 * Matrix A(size,size)
professorrodriguezse 1:5794ff4efa9a 99 * @param[out] v
professorrodriguezse 1:5794ff4efa9a 100 * Diagonal vector, v(size) = diag(A(size*size))
professorrodriguezse 1:5794ff4efa9a 101 *
professorrodriguezse 1:5794ff4efa9a 102 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 103 *
professorrodriguezse 1:5794ff4efa9a 104 * @retval
professorrodriguezse 1:5794ff4efa9a 105 * None
professorrodriguezse 1:5794ff4efa9a 106 */
professorrodriguezse 1:5794ff4efa9a 107 void gtrack_matrixGetDiag(int size, float *A, float *v)
professorrodriguezse 1:5794ff4efa9a 108 {
professorrodriguezse 1:5794ff4efa9a 109 /* v(size) = diag(A(size*size)) */
professorrodriguezse 1:5794ff4efa9a 110 int i;
professorrodriguezse 1:5794ff4efa9a 111 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 112 v[i] = A[i+i*size];
professorrodriguezse 1:5794ff4efa9a 113 }
professorrodriguezse 1:5794ff4efa9a 114 }
professorrodriguezse 1:5794ff4efa9a 115
professorrodriguezse 1:5794ff4efa9a 116 /**
professorrodriguezse 1:5794ff4efa9a 117 * @b Description
professorrodriguezse 1:5794ff4efa9a 118 * @n
professorrodriguezse 1:5794ff4efa9a 119 * This function is used to multiply two matrices.
professorrodriguezse 1:5794ff4efa9a 120 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 121 * Matrices are in row-major order
professorrodriguezse 1:5794ff4efa9a 122 *
professorrodriguezse 1:5794ff4efa9a 123 * @param[in] rows
professorrodriguezse 1:5794ff4efa9a 124 * Outer dimension, number of rows
professorrodriguezse 1:5794ff4efa9a 125 * @param[in] m
professorrodriguezse 1:5794ff4efa9a 126 * Inner dimension
professorrodriguezse 1:5794ff4efa9a 127 * @param[in] cols
professorrodriguezse 1:5794ff4efa9a 128 * Outer dimension, number of cols
professorrodriguezse 1:5794ff4efa9a 129 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 130 * Matrix A
professorrodriguezse 1:5794ff4efa9a 131 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 132 * Matrix B
professorrodriguezse 1:5794ff4efa9a 133 * @param[out] C
professorrodriguezse 1:5794ff4efa9a 134 * Matrix C(rows,cols) = A(rows,m) X B(cols,m)T
professorrodriguezse 1:5794ff4efa9a 135 *
professorrodriguezse 1:5794ff4efa9a 136 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 137 *
professorrodriguezse 1:5794ff4efa9a 138 * @retval
professorrodriguezse 1:5794ff4efa9a 139 * None
professorrodriguezse 1:5794ff4efa9a 140 */
professorrodriguezse 1:5794ff4efa9a 141 void gtrack_matrixMultiply(int rows, int m, int cols, float *A, float *B, float *C)
professorrodriguezse 1:5794ff4efa9a 142 {
professorrodriguezse 1:5794ff4efa9a 143 /* C(rows*cols) = A(rows*m)*B(m*cols) */
professorrodriguezse 1:5794ff4efa9a 144 int i,j, k;
professorrodriguezse 1:5794ff4efa9a 145 for (i = 0; i < rows; i++) {
professorrodriguezse 1:5794ff4efa9a 146 for (j = 0; j < cols; j++) {
professorrodriguezse 1:5794ff4efa9a 147 C[i*cols + j] = 0;
professorrodriguezse 1:5794ff4efa9a 148 for (k = 0; k < m; k++) {
professorrodriguezse 1:5794ff4efa9a 149 C[i*cols+j] += A[i*m+k] * B[k*cols + j];
professorrodriguezse 1:5794ff4efa9a 150 }
professorrodriguezse 1:5794ff4efa9a 151 }
professorrodriguezse 1:5794ff4efa9a 152 }
professorrodriguezse 1:5794ff4efa9a 153 }
professorrodriguezse 1:5794ff4efa9a 154
professorrodriguezse 1:5794ff4efa9a 155 /**
professorrodriguezse 1:5794ff4efa9a 156 * @b Description
professorrodriguezse 1:5794ff4efa9a 157 * @n
professorrodriguezse 1:5794ff4efa9a 158 * This function is used to multiply two matrices. Second Matrix is getting transposed first
professorrodriguezse 1:5794ff4efa9a 159 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 160 * Matrices are in row-major order
professorrodriguezse 1:5794ff4efa9a 161 *
professorrodriguezse 1:5794ff4efa9a 162 * @param[in] rows
professorrodriguezse 1:5794ff4efa9a 163 * Outer dimension, number of rows
professorrodriguezse 1:5794ff4efa9a 164 * @param[in] m
professorrodriguezse 1:5794ff4efa9a 165 * Inner dimension
professorrodriguezse 1:5794ff4efa9a 166 * @param[in] cols
professorrodriguezse 1:5794ff4efa9a 167 * Outer dimension, number of cols
professorrodriguezse 1:5794ff4efa9a 168 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 169 * Matrix A
professorrodriguezse 1:5794ff4efa9a 170 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 171 * Matrix B
professorrodriguezse 1:5794ff4efa9a 172 * @param[out] C
professorrodriguezse 1:5794ff4efa9a 173 * Matrix C(rows,cols) = A(rows,m) X B(cols,m)T
professorrodriguezse 1:5794ff4efa9a 174 *
professorrodriguezse 1:5794ff4efa9a 175 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 176 *
professorrodriguezse 1:5794ff4efa9a 177 * @retval
professorrodriguezse 1:5794ff4efa9a 178 * None
professorrodriguezse 1:5794ff4efa9a 179 */
professorrodriguezse 1:5794ff4efa9a 180 void gtrack_matrixTransposeMultiply(int rows, int m, int cols, float *A, float *B, float *C)
professorrodriguezse 1:5794ff4efa9a 181 {
professorrodriguezse 1:5794ff4efa9a 182 /* C(rows*cols) = A(rows*m)*B(cols*m)T */
professorrodriguezse 1:5794ff4efa9a 183 int i,j, k;
professorrodriguezse 1:5794ff4efa9a 184 for (i = 0; i < rows; i++) {
professorrodriguezse 1:5794ff4efa9a 185 for (j = 0; j < cols; j++) {
professorrodriguezse 1:5794ff4efa9a 186 C[i*cols + j] = 0;
professorrodriguezse 1:5794ff4efa9a 187 for (k = 0; k < m; k++) {
professorrodriguezse 1:5794ff4efa9a 188 C[i*cols+j] += A[i*m+k] * B[k + j*m];
professorrodriguezse 1:5794ff4efa9a 189 }
professorrodriguezse 1:5794ff4efa9a 190 }
professorrodriguezse 1:5794ff4efa9a 191 }
professorrodriguezse 1:5794ff4efa9a 192 }
professorrodriguezse 1:5794ff4efa9a 193
professorrodriguezse 1:5794ff4efa9a 194 /**
professorrodriguezse 1:5794ff4efa9a 195 * @b Description
professorrodriguezse 1:5794ff4efa9a 196 * @n
professorrodriguezse 1:5794ff4efa9a 197 * This function is used to multiply matrix by a scalar.
professorrodriguezse 1:5794ff4efa9a 198 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 199 * Matrices are in row-major order
professorrodriguezse 1:5794ff4efa9a 200 *
professorrodriguezse 1:5794ff4efa9a 201 * @param[in] rows
professorrodriguezse 1:5794ff4efa9a 202 * Number of rows
professorrodriguezse 1:5794ff4efa9a 203 * @param[in] cols
professorrodriguezse 1:5794ff4efa9a 204 * Number of cols
professorrodriguezse 1:5794ff4efa9a 205 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 206 * Matrix A
professorrodriguezse 1:5794ff4efa9a 207 * @param[in] c
professorrodriguezse 1:5794ff4efa9a 208 * Scalar c
professorrodriguezse 1:5794ff4efa9a 209 * @param[out] B
professorrodriguezse 1:5794ff4efa9a 210 * Matrix B(rows,cols) = A(rows,cols) * c
professorrodriguezse 1:5794ff4efa9a 211 *
professorrodriguezse 1:5794ff4efa9a 212 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 213 *
professorrodriguezse 1:5794ff4efa9a 214 * @retval
professorrodriguezse 1:5794ff4efa9a 215 * None
professorrodriguezse 1:5794ff4efa9a 216 */
professorrodriguezse 1:5794ff4efa9a 217 void gtrack_matrixScalarMultiply(int rows, int cols, float *A, float c, float *B)
professorrodriguezse 1:5794ff4efa9a 218 {
professorrodriguezse 1:5794ff4efa9a 219 /* B(rows*cols) = A(rows*cols)*C */
professorrodriguezse 1:5794ff4efa9a 220 int i;
professorrodriguezse 1:5794ff4efa9a 221 for (i = 0U; i < rows*cols; i++) {
professorrodriguezse 1:5794ff4efa9a 222 B[i] = A[i] * c;
professorrodriguezse 1:5794ff4efa9a 223 }
professorrodriguezse 1:5794ff4efa9a 224 }
professorrodriguezse 1:5794ff4efa9a 225
professorrodriguezse 1:5794ff4efa9a 226 /**
professorrodriguezse 1:5794ff4efa9a 227 * @b Description
professorrodriguezse 1:5794ff4efa9a 228 * @n
professorrodriguezse 1:5794ff4efa9a 229 * This function is used to add two matrices.
professorrodriguezse 1:5794ff4efa9a 230 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 231 * Matrices are in row-major order
professorrodriguezse 1:5794ff4efa9a 232 *
professorrodriguezse 1:5794ff4efa9a 233 * @param[in] rows
professorrodriguezse 1:5794ff4efa9a 234 * Number of rows
professorrodriguezse 1:5794ff4efa9a 235 * @param[in] cols
professorrodriguezse 1:5794ff4efa9a 236 * Number of cols
professorrodriguezse 1:5794ff4efa9a 237 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 238 * Matrix A
professorrodriguezse 1:5794ff4efa9a 239 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 240 * Matrix B
professorrodriguezse 1:5794ff4efa9a 241 * @param[out] C
professorrodriguezse 1:5794ff4efa9a 242 * Matrix C(rows,cols) = A(rows,cols) + B(rows,cols)
professorrodriguezse 1:5794ff4efa9a 243 *
professorrodriguezse 1:5794ff4efa9a 244 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 245 *
professorrodriguezse 1:5794ff4efa9a 246 * @retval
professorrodriguezse 1:5794ff4efa9a 247 * None
professorrodriguezse 1:5794ff4efa9a 248 */
professorrodriguezse 1:5794ff4efa9a 249 void gtrack_matrixAdd(int rows, int cols, float *A, float *B, float *C)
professorrodriguezse 1:5794ff4efa9a 250 {
professorrodriguezse 1:5794ff4efa9a 251 /* C(rows*cols) = A(rows*cols) + B(rows*cols) */
professorrodriguezse 1:5794ff4efa9a 252 int i;
professorrodriguezse 1:5794ff4efa9a 253 for (i = 0U; i < rows*cols; i++) {
professorrodriguezse 1:5794ff4efa9a 254 C[i] = A[i] + B[i];
professorrodriguezse 1:5794ff4efa9a 255 }
professorrodriguezse 1:5794ff4efa9a 256 }
professorrodriguezse 1:5794ff4efa9a 257
professorrodriguezse 1:5794ff4efa9a 258 /**
professorrodriguezse 1:5794ff4efa9a 259 * @b Description
professorrodriguezse 1:5794ff4efa9a 260 * @n
professorrodriguezse 1:5794ff4efa9a 261 * This function is used to subtract two matrices.
professorrodriguezse 1:5794ff4efa9a 262 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 263 * Matrices are in row-major order
professorrodriguezse 1:5794ff4efa9a 264 *
professorrodriguezse 1:5794ff4efa9a 265 * @param[in] rows
professorrodriguezse 1:5794ff4efa9a 266 * Number of rows
professorrodriguezse 1:5794ff4efa9a 267 * @param[in] cols
professorrodriguezse 1:5794ff4efa9a 268 * Number of cols
professorrodriguezse 1:5794ff4efa9a 269 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 270 * Matrix A
professorrodriguezse 1:5794ff4efa9a 271 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 272 * Matrix B
professorrodriguezse 1:5794ff4efa9a 273 * @param[out] C
professorrodriguezse 1:5794ff4efa9a 274 * Matrix C(rows,cols) = A(rows,cols) - B(rows,cols)
professorrodriguezse 1:5794ff4efa9a 275 *
professorrodriguezse 1:5794ff4efa9a 276 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 277 *
professorrodriguezse 1:5794ff4efa9a 278 * @retval
professorrodriguezse 1:5794ff4efa9a 279 * None
professorrodriguezse 1:5794ff4efa9a 280 */
professorrodriguezse 1:5794ff4efa9a 281 void gtrack_matrixSub(int rows, int cols, float *A, float *B, float *C)
professorrodriguezse 1:5794ff4efa9a 282 {
professorrodriguezse 1:5794ff4efa9a 283 /* C(rows*cols) = A(rows*cols) - B(rows*cols) */
professorrodriguezse 1:5794ff4efa9a 284 int i;
professorrodriguezse 1:5794ff4efa9a 285 for (i = 0U; i < rows*cols; i++) {
professorrodriguezse 1:5794ff4efa9a 286 C[i] = A[i] - B[i];
professorrodriguezse 1:5794ff4efa9a 287 }
professorrodriguezse 1:5794ff4efa9a 288 }
professorrodriguezse 1:5794ff4efa9a 289
professorrodriguezse 1:5794ff4efa9a 290 /**
professorrodriguezse 1:5794ff4efa9a 291 * @b Description
professorrodriguezse 1:5794ff4efa9a 292 * @n
professorrodriguezse 1:5794ff4efa9a 293 * This function is used to force matrix symmetry by averaging off-diagonal elements
professorrodriguezse 1:5794ff4efa9a 294 * Matrices are squared, real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 295 * Matrices are in row-major order
professorrodriguezse 1:5794ff4efa9a 296 *
professorrodriguezse 1:5794ff4efa9a 297 * @param[in] m (m=rows=cols)
professorrodriguezse 1:5794ff4efa9a 298 * Number of rows and cols
professorrodriguezse 1:5794ff4efa9a 299 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 300 * Matrix A
professorrodriguezse 1:5794ff4efa9a 301 * @param[out] B
professorrodriguezse 1:5794ff4efa9a 302 * Matrix B
professorrodriguezse 1:5794ff4efa9a 303 *
professorrodriguezse 1:5794ff4efa9a 304 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 305 *
professorrodriguezse 1:5794ff4efa9a 306 * @retval
professorrodriguezse 1:5794ff4efa9a 307 * None
professorrodriguezse 1:5794ff4efa9a 308 */
professorrodriguezse 1:5794ff4efa9a 309 void gtrack_matrixMakeSymmetrical(int m, float *A, float *B)
professorrodriguezse 1:5794ff4efa9a 310 {
professorrodriguezse 1:5794ff4efa9a 311 /* Make square matrix symmetrical by averaging off-diagonal elements */
professorrodriguezse 1:5794ff4efa9a 312 int i,j;
professorrodriguezse 1:5794ff4efa9a 313 for (i = 0U; i < m-1; i++) {
professorrodriguezse 1:5794ff4efa9a 314 B[i*m + i] = A[i*m + i];
professorrodriguezse 1:5794ff4efa9a 315 for (j = i+1; j < m; j++) {
professorrodriguezse 1:5794ff4efa9a 316 B[i*m+j] = B[j*m+i] = 0.5f*(A[i*m+j]+A[j*m+i]);
professorrodriguezse 1:5794ff4efa9a 317 }
professorrodriguezse 1:5794ff4efa9a 318 }
professorrodriguezse 1:5794ff4efa9a 319 B[i*m + i] = A[i*m + i];
professorrodriguezse 1:5794ff4efa9a 320 }
professorrodriguezse 1:5794ff4efa9a 321
professorrodriguezse 1:5794ff4efa9a 322
professorrodriguezse 1:5794ff4efa9a 323 /**
professorrodriguezse 1:5794ff4efa9a 324 * @b Description
professorrodriguezse 1:5794ff4efa9a 325 * @n
professorrodriguezse 1:5794ff4efa9a 326 * This function is used to initialise vector to a value
professorrodriguezse 1:5794ff4efa9a 327 *
professorrodriguezse 1:5794ff4efa9a 328 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 329 * Size of vector
professorrodriguezse 1:5794ff4efa9a 330 * @param[in] value
professorrodriguezse 1:5794ff4efa9a 331 * value to set
professorrodriguezse 1:5794ff4efa9a 332 * @param[out] A
professorrodriguezse 1:5794ff4efa9a 333 * Vector A
professorrodriguezse 1:5794ff4efa9a 334 *
professorrodriguezse 1:5794ff4efa9a 335 * A(size) = ones(size,1)*value
professorrodriguezse 1:5794ff4efa9a 336 *
professorrodriguezse 1:5794ff4efa9a 337 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 338 *
professorrodriguezse 1:5794ff4efa9a 339 * @retval
professorrodriguezse 1:5794ff4efa9a 340 * None
professorrodriguezse 1:5794ff4efa9a 341 */
professorrodriguezse 1:5794ff4efa9a 342 void gtrack_vectorInit(int size, float value, float *A)
professorrodriguezse 1:5794ff4efa9a 343 {
professorrodriguezse 1:5794ff4efa9a 344 /* A(size) = ones(size,1)*value */
professorrodriguezse 1:5794ff4efa9a 345 int i;
professorrodriguezse 1:5794ff4efa9a 346
professorrodriguezse 1:5794ff4efa9a 347 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 348 A[i] = value;
professorrodriguezse 1:5794ff4efa9a 349 }
professorrodriguezse 1:5794ff4efa9a 350 }
professorrodriguezse 1:5794ff4efa9a 351 /**
professorrodriguezse 1:5794ff4efa9a 352 * @b Description
professorrodriguezse 1:5794ff4efa9a 353 * @n
professorrodriguezse 1:5794ff4efa9a 354 * This function adds two vectors
professorrodriguezse 1:5794ff4efa9a 355 * Vectors are real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 356 *
professorrodriguezse 1:5794ff4efa9a 357 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 358 * Size of vector
professorrodriguezse 1:5794ff4efa9a 359 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 360 * Vector A
professorrodriguezse 1:5794ff4efa9a 361 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 362 * Vector B
professorrodriguezse 1:5794ff4efa9a 363 * @param[out] C
professorrodriguezse 1:5794ff4efa9a 364 * Vector C = A + B;
professorrodriguezse 1:5794ff4efa9a 365 *
professorrodriguezse 1:5794ff4efa9a 366 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 367 *
professorrodriguezse 1:5794ff4efa9a 368 * @retval
professorrodriguezse 1:5794ff4efa9a 369 * None
professorrodriguezse 1:5794ff4efa9a 370 */
professorrodriguezse 1:5794ff4efa9a 371 void gtrack_vectorAdd(int size, float *A, float *B, float *C)
professorrodriguezse 1:5794ff4efa9a 372 {
professorrodriguezse 1:5794ff4efa9a 373 int i;
professorrodriguezse 1:5794ff4efa9a 374 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 375 C[i] = A[i] + B[i];
professorrodriguezse 1:5794ff4efa9a 376 }
professorrodriguezse 1:5794ff4efa9a 377 }
professorrodriguezse 1:5794ff4efa9a 378
professorrodriguezse 1:5794ff4efa9a 379 /**
professorrodriguezse 1:5794ff4efa9a 380 * @b Description
professorrodriguezse 1:5794ff4efa9a 381 * @n
professorrodriguezse 1:5794ff4efa9a 382 * This function subtracts two vectors
professorrodriguezse 1:5794ff4efa9a 383 * Vectors are real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 384 *
professorrodriguezse 1:5794ff4efa9a 385 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 386 * Size of vectors
professorrodriguezse 1:5794ff4efa9a 387 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 388 * Vector A
professorrodriguezse 1:5794ff4efa9a 389 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 390 * Vector B
professorrodriguezse 1:5794ff4efa9a 391 * @param[out] C
professorrodriguezse 1:5794ff4efa9a 392 * Vector C = A - B;
professorrodriguezse 1:5794ff4efa9a 393 *
professorrodriguezse 1:5794ff4efa9a 394 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 395 *
professorrodriguezse 1:5794ff4efa9a 396 * @retval
professorrodriguezse 1:5794ff4efa9a 397 * None
professorrodriguezse 1:5794ff4efa9a 398 */
professorrodriguezse 1:5794ff4efa9a 399 void gtrack_vectorSub(int size, float *A, float *B, float *C)
professorrodriguezse 1:5794ff4efa9a 400 {
professorrodriguezse 1:5794ff4efa9a 401 int i;
professorrodriguezse 1:5794ff4efa9a 402 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 403 C[i] = A[i] - B[i];
professorrodriguezse 1:5794ff4efa9a 404 }
professorrodriguezse 1:5794ff4efa9a 405 }
professorrodriguezse 1:5794ff4efa9a 406
professorrodriguezse 1:5794ff4efa9a 407 /**
professorrodriguezse 1:5794ff4efa9a 408 * @b Description
professorrodriguezse 1:5794ff4efa9a 409 * @n
professorrodriguezse 1:5794ff4efa9a 410 * This function multiplies vector by scalar
professorrodriguezse 1:5794ff4efa9a 411 * Vectors are real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 412 * Scalar is real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 413 *
professorrodriguezse 1:5794ff4efa9a 414 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 415 * Size of vector
professorrodriguezse 1:5794ff4efa9a 416 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 417 * Vector A
professorrodriguezse 1:5794ff4efa9a 418 * @param[in] c
professorrodriguezse 1:5794ff4efa9a 419 * Scalar c
professorrodriguezse 1:5794ff4efa9a 420 * @param[out] B
professorrodriguezse 1:5794ff4efa9a 421 * Vector B = A*c;
professorrodriguezse 1:5794ff4efa9a 422 *
professorrodriguezse 1:5794ff4efa9a 423 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 424 *
professorrodriguezse 1:5794ff4efa9a 425 * @retval
professorrodriguezse 1:5794ff4efa9a 426 * None
professorrodriguezse 1:5794ff4efa9a 427 */
professorrodriguezse 1:5794ff4efa9a 428 void gtrack_vectorScalarMul(int size, float *A, float c, float *B)
professorrodriguezse 1:5794ff4efa9a 429 {
professorrodriguezse 1:5794ff4efa9a 430 int i;
professorrodriguezse 1:5794ff4efa9a 431 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 432 B[i] = A[i]*c;
professorrodriguezse 1:5794ff4efa9a 433 }
professorrodriguezse 1:5794ff4efa9a 434 }
professorrodriguezse 1:5794ff4efa9a 435
professorrodriguezse 1:5794ff4efa9a 436 /**
professorrodriguezse 1:5794ff4efa9a 437 * @b Description
professorrodriguezse 1:5794ff4efa9a 438 * @n
professorrodriguezse 1:5794ff4efa9a 439 * This function performs multiplies vector by scalar and accumulates the results
professorrodriguezse 1:5794ff4efa9a 440 * Vectors are real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 441 * Scalar is real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 442 *
professorrodriguezse 1:5794ff4efa9a 443 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 444 * Size of vector
professorrodriguezse 1:5794ff4efa9a 445 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 446 * Vector A
professorrodriguezse 1:5794ff4efa9a 447 * @param[in] c
professorrodriguezse 1:5794ff4efa9a 448 * Scalar c
professorrodriguezse 1:5794ff4efa9a 449 * @param[in, out] B
professorrodriguezse 1:5794ff4efa9a 450 * Vector B = B + A*c;
professorrodriguezse 1:5794ff4efa9a 451 *
professorrodriguezse 1:5794ff4efa9a 452 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 453 *
professorrodriguezse 1:5794ff4efa9a 454 * @retval
professorrodriguezse 1:5794ff4efa9a 455 * None
professorrodriguezse 1:5794ff4efa9a 456 */
professorrodriguezse 1:5794ff4efa9a 457 void gtrack_vectorScalarMulAcc(int size, float *A, float c, float *B)
professorrodriguezse 1:5794ff4efa9a 458 {
professorrodriguezse 1:5794ff4efa9a 459 int i;
professorrodriguezse 1:5794ff4efa9a 460 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 461 B[i] = B[i] + A[i]*c;
professorrodriguezse 1:5794ff4efa9a 462 }
professorrodriguezse 1:5794ff4efa9a 463 }
professorrodriguezse 1:5794ff4efa9a 464
professorrodriguezse 1:5794ff4efa9a 465 /**
professorrodriguezse 1:5794ff4efa9a 466 * @b Description
professorrodriguezse 1:5794ff4efa9a 467 * @n
professorrodriguezse 1:5794ff4efa9a 468 * This function performs IIR vector filtering
professorrodriguezse 1:5794ff4efa9a 469 * Vectors are real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 470 * Alpha is real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 471 *
professorrodriguezse 1:5794ff4efa9a 472 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 473 * Size of vector
professorrodriguezse 1:5794ff4efa9a 474 * @param[in, out] A
professorrodriguezse 1:5794ff4efa9a 475 * Vector A
professorrodriguezse 1:5794ff4efa9a 476 * @param[in] alpha
professorrodriguezse 1:5794ff4efa9a 477 * Weighting factor for new information, (0<=alpha<=1.0f)
professorrodriguezse 1:5794ff4efa9a 478 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 479 * New information vector B
professorrodriguezse 1:5794ff4efa9a 480 *
professorrodriguezse 1:5794ff4efa9a 481 * Vector A = A*(1.0f-alpha) + B*alpha;
professorrodriguezse 1:5794ff4efa9a 482 *
professorrodriguezse 1:5794ff4efa9a 483 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 484 *
professorrodriguezse 1:5794ff4efa9a 485 * @retval
professorrodriguezse 1:5794ff4efa9a 486 * None
professorrodriguezse 1:5794ff4efa9a 487 */
professorrodriguezse 1:5794ff4efa9a 488 void gtrack_vectorFilter(int size, float *A, float alpha, float *B)
professorrodriguezse 1:5794ff4efa9a 489 {
professorrodriguezse 1:5794ff4efa9a 490 int i;
professorrodriguezse 1:5794ff4efa9a 491 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 492 A[i] = A[i]*(1.0f - alpha) + B[i]*alpha;
professorrodriguezse 1:5794ff4efa9a 493 }
professorrodriguezse 1:5794ff4efa9a 494 }
professorrodriguezse 1:5794ff4efa9a 495
professorrodriguezse 1:5794ff4efa9a 496
professorrodriguezse 1:5794ff4efa9a 497 /**
professorrodriguezse 1:5794ff4efa9a 498 * @b Description
professorrodriguezse 1:5794ff4efa9a 499 * @n
professorrodriguezse 1:5794ff4efa9a 500 * This function accumulates covariance matrix with variances from input vector and mean
professorrodriguezse 1:5794ff4efa9a 501 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 502 * Vectors are real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 503 *
professorrodriguezse 1:5794ff4efa9a 504 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 505 * Size of square matrix
professorrodriguezse 1:5794ff4efa9a 506 * @param[in] A
professorrodriguezse 1:5794ff4efa9a 507 * Matrix A
professorrodriguezse 1:5794ff4efa9a 508 * @param[in] v
professorrodriguezse 1:5794ff4efa9a 509 * Vector v
professorrodriguezse 1:5794ff4efa9a 510 * @param[in] mean
professorrodriguezse 1:5794ff4efa9a 511 * Vector mean
professorrodriguezse 1:5794ff4efa9a 512 *
professorrodriguezse 1:5794ff4efa9a 513 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 514 *
professorrodriguezse 1:5794ff4efa9a 515 * @retval
professorrodriguezse 1:5794ff4efa9a 516 * None
professorrodriguezse 1:5794ff4efa9a 517 */
professorrodriguezse 1:5794ff4efa9a 518 void gtrack_matrixCovAcc(int size, float *A, float *v, float *mean)
professorrodriguezse 1:5794ff4efa9a 519 {
professorrodriguezse 1:5794ff4efa9a 520 int i,j;
professorrodriguezse 1:5794ff4efa9a 521 float d1, d2;
professorrodriguezse 1:5794ff4efa9a 522
professorrodriguezse 1:5794ff4efa9a 523 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 524 d1 = v[i]-mean[i];
professorrodriguezse 1:5794ff4efa9a 525 for (j = i; j < size; j++) {
professorrodriguezse 1:5794ff4efa9a 526 d2 = v[j]-mean[j];
professorrodriguezse 1:5794ff4efa9a 527 A[i*size+j] += d1*d2;
professorrodriguezse 1:5794ff4efa9a 528 }
professorrodriguezse 1:5794ff4efa9a 529 }
professorrodriguezse 1:5794ff4efa9a 530 }
professorrodriguezse 1:5794ff4efa9a 531
professorrodriguezse 1:5794ff4efa9a 532 /**
professorrodriguezse 1:5794ff4efa9a 533 * @b Description
professorrodriguezse 1:5794ff4efa9a 534 * @n
professorrodriguezse 1:5794ff4efa9a 535 * This function normalizes covariance matrix
professorrodriguezse 1:5794ff4efa9a 536 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 537 *
professorrodriguezse 1:5794ff4efa9a 538 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 539 * Size of square matrix
professorrodriguezse 1:5794ff4efa9a 540 * @param[in,out] A
professorrodriguezse 1:5794ff4efa9a 541 * Matrix A
professorrodriguezse 1:5794ff4efa9a 542 * @param[in] num
professorrodriguezse 1:5794ff4efa9a 543 * Number of measurments num
professorrodriguezse 1:5794ff4efa9a 544 *
professorrodriguezse 1:5794ff4efa9a 545 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 546 *
professorrodriguezse 1:5794ff4efa9a 547 * @retval
professorrodriguezse 1:5794ff4efa9a 548 * None
professorrodriguezse 1:5794ff4efa9a 549 */
professorrodriguezse 1:5794ff4efa9a 550 void gtrack_matrixCovNormalize(int size, float *A, int num)
professorrodriguezse 1:5794ff4efa9a 551 {
professorrodriguezse 1:5794ff4efa9a 552 int i,j;
professorrodriguezse 1:5794ff4efa9a 553 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 554 A[i*size+i] /= num;
professorrodriguezse 1:5794ff4efa9a 555 for (j = i+1; j < size; j++) {
professorrodriguezse 1:5794ff4efa9a 556 A[i*size+j] /= num;
professorrodriguezse 1:5794ff4efa9a 557 A[i+j*size] = A[i*size+j];
professorrodriguezse 1:5794ff4efa9a 558 }
professorrodriguezse 1:5794ff4efa9a 559 }
professorrodriguezse 1:5794ff4efa9a 560 }
professorrodriguezse 1:5794ff4efa9a 561 /**
professorrodriguezse 1:5794ff4efa9a 562 * @b Description
professorrodriguezse 1:5794ff4efa9a 563 * @n
professorrodriguezse 1:5794ff4efa9a 564 * This function filters covariance matrix
professorrodriguezse 1:5794ff4efa9a 565 * Matrices are all real, single precision floating point.
professorrodriguezse 1:5794ff4efa9a 566 *
professorrodriguezse 1:5794ff4efa9a 567 * @param[in] size
professorrodriguezse 1:5794ff4efa9a 568 * Size of square matrix
professorrodriguezse 1:5794ff4efa9a 569 * @param[in,out] A
professorrodriguezse 1:5794ff4efa9a 570 * Matrix A
professorrodriguezse 1:5794ff4efa9a 571 * @param[in] B
professorrodriguezse 1:5794ff4efa9a 572 * Matrix B
professorrodriguezse 1:5794ff4efa9a 573 * @param[in] alpha
professorrodriguezse 1:5794ff4efa9a 574 * Filtering coefficient alpha
professorrodriguezse 1:5794ff4efa9a 575 * Matrix A = (1-alpha)*A + alpha*B
professorrodriguezse 1:5794ff4efa9a 576 * \ingroup GTRACK_ALG_MATH_FUNCTION
professorrodriguezse 1:5794ff4efa9a 577 *
professorrodriguezse 1:5794ff4efa9a 578 * @retval
professorrodriguezse 1:5794ff4efa9a 579 * None
professorrodriguezse 1:5794ff4efa9a 580 */
professorrodriguezse 1:5794ff4efa9a 581
professorrodriguezse 1:5794ff4efa9a 582 void gtrack_matrixCovFilter(int size, float *A, float *B, float alpha)
professorrodriguezse 1:5794ff4efa9a 583 {
professorrodriguezse 1:5794ff4efa9a 584 int i,j;
professorrodriguezse 1:5794ff4efa9a 585 for (i = 0U; i < size; i++) {
professorrodriguezse 1:5794ff4efa9a 586 A[i*size+i] = (1-alpha)*A[i*size+i] + alpha*B[i*size+i];
professorrodriguezse 1:5794ff4efa9a 587 for (j = i+1; j < size; j++) {
professorrodriguezse 1:5794ff4efa9a 588 A[i*size+j] = (1-alpha)*A[i*size+j] + alpha*B[i*size+j];
professorrodriguezse 1:5794ff4efa9a 589 A[i+j*size] = A[i*size+j];
professorrodriguezse 1:5794ff4efa9a 590 }
professorrodriguezse 1:5794ff4efa9a 591 }
professorrodriguezse 1:5794ff4efa9a 592 }
professorrodriguezse 1:5794ff4efa9a 593
professorrodriguezse 1:5794ff4efa9a 594 void gtrack_matrixPrint(int rows, int cols, float *A)
professorrodriguezse 1:5794ff4efa9a 595 {
professorrodriguezse 1:5794ff4efa9a 596 int i,j;
professorrodriguezse 1:5794ff4efa9a 597 for (i = 0U; i < rows; i++)
professorrodriguezse 1:5794ff4efa9a 598 {
professorrodriguezse 1:5794ff4efa9a 599 for (j = 0U; j < cols-1; j++)
professorrodriguezse 1:5794ff4efa9a 600 {
professorrodriguezse 1:5794ff4efa9a 601 printf("%6.4f\t", A[i*cols +j]);
professorrodriguezse 1:5794ff4efa9a 602 }
professorrodriguezse 1:5794ff4efa9a 603 printf("%6.4f\n\r", A[i*cols +j]);
professorrodriguezse 1:5794ff4efa9a 604 }
professorrodriguezse 1:5794ff4efa9a 605 printf("\n\r");
professorrodriguezse 1:5794ff4efa9a 606 }