MRTKLIB 0.7.14
Modernized GNSS Positioning Library
Loading...
Searching...
No Matches
mrtk_mat.h
Go to the documentation of this file.
1/*------------------------------------------------------------------------------
2 * mrtk_mat.h : matrix and vector type definitions and functions
3 *
4 * Copyright (C) 2026 H.SHIONO (MRTKLIB Project)
5 * Copyright (C) 2023-2025 Cabinet Office, Japan
6 * Copyright (C) 2024-2025 Lighthouse Technology & Consulting Co. Ltd.
7 * Copyright (C) 2023-2025 Japan Aerospace Exploration Agency
8 * Copyright (C) 2023-2025 TOSHIBA ELECTRONIC TECHNOLOGIES CORPORATION
9 * Copyright (C) 2015- Mitsubishi Electric Corp.
10 * Copyright (C) 2014 Geospatial Information Authority of Japan
11 * Copyright (C) 2014 T.SUZUKI
12 * Copyright (C) 2007-2023 T.TAKASU
13 *
14 * SPDX-License-Identifier: BSD-2-Clause
15 *----------------------------------------------------------------------------*/
16/**
17 * @file mrtk_mat.h
18 * @brief MRTKLIB Matrix Module — Matrix/vector operations and estimation.
19 *
20 * This header provides matrix and vector allocation, arithmetic, linear
21 * algebra (LU decomposition, matrix inversion, linear solve), and
22 * estimation algorithms (least squares, Kalman filter, smoother).
23 *
24 * @note All functions in this module are pure cut-and-paste extractions
25 * from rtkcmn.c with zero algorithmic changes.
26 */
27#ifndef MRTK_MAT_H
28#define MRTK_MAT_H
29
30#ifdef __cplusplus
31extern "C" {
32#endif
33
34#include <stdio.h>
35
36#include "mrtklib/mrtk_foundation.h"
37
38/*============================================================================
39 * Fatal Callback
40 *===========================================================================*/
41
42/** @brief Fatal callback function type for allocation failures. */
43typedef void fatalfunc_t(const char*);
44
45/**
46 * @brief Add fatal callback function for mat(), zeros(), imat().
47 * @param[in] func Callback function
48 */
49void add_fatal(fatalfunc_t* func);
50
51/*============================================================================
52 * Matrix Allocation
53 *===========================================================================*/
54
55/**
56 * @brief Allocate memory for a double matrix.
57 * @param[in] n Number of rows
58 * @param[in] m Number of columns
59 * @return Matrix pointer (NULL if n<=0 or m<=0)
60 */
61double* mat(int n, int m);
62
63/**
64 * @brief Allocate memory for an integer matrix.
65 * @param[in] n Number of rows
66 * @param[in] m Number of columns
67 * @return Matrix pointer (NULL if n<=0 or m<=0)
68 */
69int* imat(int n, int m);
70
71/**
72 * @brief Allocate a new zero matrix.
73 * @param[in] n Number of rows
74 * @param[in] m Number of columns
75 * @return Matrix pointer (NULL if n<=0 or m<=0)
76 */
77double* zeros(int n, int m);
78
79/**
80 * @brief Generate a new identity matrix.
81 * @param[in] n Number of rows and columns
82 * @return Matrix pointer (NULL if n<=0)
83 */
84double* eye(int n);
85
86/*============================================================================
87 * Vector Operations
88 *===========================================================================*/
89
90/**
91 * @brief Inner product of vectors.
92 * @param[in] a Vector a (n x 1)
93 * @param[in] b Vector b (n x 1)
94 * @param[in] n Size of vectors
95 * @return a' * b
96 */
97double dot(const double* a, const double* b, int n);
98
99/**
100 * @brief Euclidean norm of vector.
101 * @param[in] a Vector a (n x 1)
102 * @param[in] n Size of vector
103 * @return || a ||
104 */
105double norm(const double* a, int n);
106
107/**
108 * @brief Cross product of 3D vectors.
109 * @param[in] a Vector a (3 x 1)
110 * @param[in] b Vector b (3 x 1)
111 * @param[out] c Cross product a x b (3 x 1)
112 */
113void cross3(const double* a, const double* b, double* c);
114
115/**
116 * @brief Normalize 3D vector.
117 * @param[in] a Vector a (3 x 1)
118 * @param[out] b Normalized vector (3 x 1), || b || = 1
119 * @return Status (1:ok, 0:error)
120 */
121int normv3(const double* a, double* b);
122
123/*============================================================================
124 * Matrix Operations
125 *===========================================================================*/
126
127/**
128 * @brief Copy matrix.
129 * @param[out] A Destination matrix (n x m)
130 * @param[in] B Source matrix (n x m)
131 * @param[in] n Number of rows
132 * @param[in] m Number of columns
133 */
134void matcpy(double* A, const double* B, int n, int m);
135
136/**
137 * @brief Multiply matrix by matrix (C = alpha*A*B + beta*C).
138 * @param[in] tr Transpose flags ("N":normal, "T":transpose)
139 * @param[in] n,k,m Size of (transposed) matrix A, B
140 * @param[in] alpha Alpha coefficient
141 * @param[in] A,B (Transposed) matrix A (n x m), B (m x k)
142 * @param[in] beta Beta coefficient
143 * @param[in,out] C Matrix C (n x k)
144 */
145void matmul(const char* tr, int n, int k, int m, double alpha, const double* A, const double* B, double beta,
146 double* C);
147
148/**
149 * @brief Inverse of matrix (A = A^-1).
150 * @param[in,out] A Matrix (n x n)
151 * @param[in] n Size of matrix
152 * @return Status (0:ok, <0:error)
153 */
154int matinv(double* A, int n);
155
156/**
157 * @brief Solve linear equation (X = A\Y or X = A'\Y).
158 * @param[in] tr Transpose flag ("N":normal, "T":transpose)
159 * @param[in] A Input matrix A (n x n)
160 * @param[in] Y Input matrix Y (n x m)
161 * @param[in] n,m Size of matrix A, Y
162 * @param[out] X Solution X = A\Y or X = A'\Y (n x m)
163 * @return Status (0:ok, <0:error)
164 * @note Matrix stored by column-major order (Fortran convention).
165 * X can be same as Y.
166 */
167int solve(const char* tr, const double* A, const double* Y, int n, int m, double* X);
168
169/*============================================================================
170 * Estimation Algorithms
171 *===========================================================================*/
172
173/**
174 * @brief Least square estimation by solving normal equation.
175 *
176 * Solves x = (A*A')^-1 * A * y.
177 *
178 * @param[in] A Transpose of (weighted) design matrix (n x m)
179 * @param[in] y (Weighted) measurements (m x 1)
180 * @param[in] n Number of parameters
181 * @param[in] m Number of measurements (n <= m)
182 * @param[out] x Estimated parameters (n x 1)
183 * @param[out] Q Estimated parameters covariance matrix (n x n)
184 * @return Status (0:ok, <0:error)
185 * @note For weighted least square, replace A and y by A*w and w*y (w=W^(1/2)).
186 * Matrix stored by column-major order (Fortran convention).
187 */
188int lsq(const double* A, const double* y, int n, int m, double* x, double* Q);
189
190/**
191 * @brief Kalman filter state update.
192 *
193 * K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P
194 *
195 * @param[in,out] x States vector (n x 1)
196 * @param[in,out] P Covariance matrix of states (n x n)
197 * @param[in] H Transpose of design matrix (n x m)
198 * @param[in] v Innovation (measurement - model) (m x 1)
199 * @param[in] R Covariance matrix of measurement error (m x m)
200 * @param[in] n Number of states
201 * @param[in] m Number of measurements
202 * @return Status (0:ok, <0:error)
203 * @note Matrix stored by column-major order (Fortran convention).
204 * If state x[i]==0.0, does not update state x[i]/P[i+i*n].
205 */
206int filter(double* x, double* P, const double* H, const double* v, const double* R, int n, int m);
207
208/**
209 * @brief Combine forward and backward filters by fixed-interval smoother.
210 *
211 * xs=Qs*(Qf^-1*xf+Qb^-1*xb), Qs=(Qf^-1+Qb^-1)^-1
212 *
213 * @param[in] xf Forward solutions (n x 1)
214 * @param[in] Qf Forward solutions covariance matrix (n x n)
215 * @param[in] xb Backward solutions (n x 1)
216 * @param[in] Qb Backward solutions covariance matrix (n x n)
217 * @param[in] n Number of solutions
218 * @param[out] xs Smoothed solutions (n x 1)
219 * @param[out] Qs Smoothed solutions covariance matrix (n x n)
220 * @return Status (0:ok, <0:error)
221 * @note Matrix stored by column-major order (Fortran convention).
222 */
223int smoother(const double* xf, const double* Qf, const double* xb, const double* Qb, int n, double* xs, double* Qs);
224
225/*============================================================================
226 * Matrix Output
227 *===========================================================================*/
228
229/**
230 * @brief Print matrix to file pointer.
231 * @param[in] A Matrix A (n x m)
232 * @param[in] n,m Number of rows and columns
233 * @param[in] p,q Total columns, columns under decimal point
234 * @param[in] fp Output file pointer
235 * @note Matrix stored by column-major order (Fortran convention).
236 */
237void matfprint(const double* A, int n, int m, int p, int q, FILE* fp);
238
239/**
240 * @brief Print matrix to stdout.
241 * @param[in] A Matrix A (n x m)
242 * @param[in] n,m Number of rows and columns
243 * @param[in] p,q Total columns, columns under decimal point
244 */
245void matprint(const double* A, int n, int m, int p, int q);
246
247#ifdef __cplusplus
248}
249#endif
250
251#endif /* MRTK_MAT_H */
mrtk_foundation.h
MRTKLIB Foundation — Common types, standard includes, and system constants (MAXSAT,...
mat
double * mat(int n, int m)
Allocate memory for a double matrix.
Definition mrtk_mat.c:76
smoother
int smoother(const double *xf, const double *Qf, const double *xb, const double *Qb, int n, double *xs, double *Qs)
Combine forward and backward filters by fixed-interval smoother.
Definition mrtk_mat.c:579
matinv
int matinv(double *A, int n)
Inverse of matrix (A = A^-1).
Definition mrtk_mat.c:421
zeros
double * zeros(int n, int m)
Allocate a new zero matrix.
Definition mrtk_mat.c:108
lsq
int lsq(const double *A, const double *y, int n, int m, double *x, double *Q)
Least square estimation by solving normal equation.
Definition mrtk_mat.c:470
matcpy
void matcpy(double *A, const double *B, int n, int m)
Copy matrix.
Definition mrtk_mat.c:205
matfprint
void matfprint(const double *A, int n, int m, int p, int q, FILE *fp)
Print matrix to file pointer.
solve
int solve(const char *tr, const double *A, const double *Y, int n, int m, double *X)
Solve linear equation (X = A\Y or X = A'\Y).
Definition mrtk_mat.c:443
cross3
void cross3(const double *a, const double *b, double *c)
Cross product of 3D vectors.
Definition mrtk_mat.c:172
imat
int * imat(int n, int m)
Allocate memory for an integer matrix.
Definition mrtk_mat.c:92
filter
int filter(double *x, double *P, const double *H, const double *v, const double *R, int n, int m)
Kalman filter state update.
Definition mrtk_mat.c:524
fatalfunc_t
void fatalfunc_t(const char *)
Fatal callback function type for allocation failures.
Definition mrtk_mat.h:43
matprint
void matprint(const double *A, int n, int m, int p, int q)
Print matrix to stdout.
add_fatal
void add_fatal(fatalfunc_t *func)
Add fatal callback function for mat(), zeros(), imat().
Definition mrtk_mat.c:65
norm
double norm(const double *a, int n)
Euclidean norm of vector.
Definition mrtk_mat.c:165
normv3
int normv3(const double *a, double *b)
Normalize 3D vector.
Definition mrtk_mat.c:183
matmul
void matmul(const char *tr, int n, int k, int m, double alpha, const double *A, const double *B, double beta, double *C)
Multiply matrix by matrix (C = alpha*A*B + beta*C).
Definition mrtk_mat.c:310
dot
double dot(const double *a, const double *b, int n)
Inner product of vectors.
Definition mrtk_mat.c:151
eye
double * eye(int n)
Generate a new identity matrix.
Definition mrtk_mat.c:129