1. 动态创建 double 矩阵
首先进行参数的合法性检查，如果 `n` 或者 `m` 小于等于 0，函数直接返回 `NULL`，表示参数不合法，无法分配数组。然后尝试分配内存，使用 `malloc` 函数动态分配 `n*m` 个 `double` 类型的空间。如果分配失败（即 `malloc` 返回 `NULL`），则调用 `fatalerr` 函数报告内存分配错误，并输出具体的 `n` 和 `m` 的值。如果内存分配成功，则将分配得到的指针 `p` 返回给调用者。
/* new matrix ------------------------------------------------------------------
* allocate memory of matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *mat(int n, int m) {
double *p;
if (n <= 0 || m <= 0) return NULL;
if (!(p = (double *)malloc(sizeof(double) * n * m))) {
fatalerr("matrix memory allocation error: n=%d, m=%d\n", n, m);
}
return p;
}
2. 动态创建 int 矩阵：`imat()`
其创建过程与 `mat()` 矩阵的创建过程一样。
/*
* Function to allocate memory for an integer matrix.
*
* Arguments:
* int n - Number of rows
* int m - Number of columns
*
* Returns:
* int* - Pointer to the allocated matrix (if n<=0 or m<=0, returns NULL)
*
*-----------------------------------------------------------------------------*/
extern int* imat(int n, int m) {
int* p;
// Check for valid matrix dimensions
if (n <= 0 || m <= 0) {
return NULL;
}
// Allocate memory for the matrix
p = (int*)malloc(sizeof(int) * n * m);
if (!p) {
fatalerr("integer matrix memory allocation error: n=%d, m=%d\n", n, m);
}
return p;
}
3. 创建全零矩阵
/* zero matrix -----------------------------------------------------------------
* generate new zero matrix
* args : int n,m I number of rows and columns of matrix
* return : matrix pointer (if n<=0 or m<=0, return NULL)
*-----------------------------------------------------------------------------*/
extern double *zeros(int n, int m) {
double *p;
#if NOCALLOC
if ((p = mat(n, m)))
for (n = n * m - 1; n >= 0; n--)
p[n] = 0.0;
#else
if (n <= 0 || m <= 0) return NULL;
if (!(p = (double *)calloc(sizeof(double), n * m))) {
fatalerr("matrix memory allocation error: n=%d, m=%d\n", n, m);
}
#endif
return p;
}
4. 单位矩阵
extern double *eye(int n) {
double *p;
int i;
if ((p = zeros(n, n)))
for (i = 0; i < n; i++)
p[i + i * n] = 1.0;
return p;
}
5. 向量内积
/* inner product ---------------------------------------------------------------
* inner product of vectors
* args : double *a,*b I vector a, b (n x 1)
* int n I size of vector a, b
* return : a'*b
*-----------------------------------------------------------------------------*/
extern double dot(const double *a, const double *b, int n) {
double c = 0.0;
while (--n >= 0)
c += a[n] * b[n];
return c;
}
6. 向量的欧几里德范数
extern double norm(const double *a, int n) {
return sqrt(dot(a, a, n));
}
7. 三维矢量的外积
extern void cross3(const double *a, const double *b, double *c) {
c[0] = a[1] * b[2] - a[2] * b[1];
c[1] = a[2] * b[0] - a[0] * b[2];
c[2] = a[0] * b[1] - a[1] * b[0];
}
8. 归一化三维矢量
归一化三维矢量指的是将一个三维向量除以其模长，使其变为单位向量的过程。
extern int normv3(const double *a, double *b) {
double r;
if ((r = norm(a, 3)) <= 0.0) return 0;
b[0] = a[0] / r;
b[1] = a[1] / r;
b[2] = a[2] / r;
return 1;
}
9. 复制矩阵
extern void matcpy(double *A, const double *B, int n, int m) {
memcpy(A, B, sizeof(double) * n * m);
}
10. LU 分解
static int ludcmp(double *A, int n, int *indx, double *d) {
double big, s, tmp, *vv = mat(n, 1);
int i, imax = 0, j, k;
*d = 1.0;
for (i = 0; i < n; i++) {
big = 0.0;
for (j = 0; j < n; j++)
if ((tmp = fabs(A[i + j * n])) > big)
big = tmp;
if (big > 0.0)
vv[i] = 1.0 / big;
else {
free(vv);
return -1;
}
}
for (j = 0; j < n; j++) {
for (i = 0; i < j; i++) {
s = A[i + j * n];
for (k = 0; k < i; k++)
s -= A[i + k * n] * A[k + j * n];
A[i + j * n] = s;
}
big = 0.0;
for (i = j; i < n; i++) {
s = A[i + j * n];
for (k = 0; k < j; k++)
s -= A[i + k * n] * A[k + j * n];
A[i + j * n] = s;
if ((tmp = vv[i] * fabs(s)) >= big) {
big = tmp;
imax = i;
}
}
if (j != imax) {
for (k = 0; k < n; k++) {
tmp = A[imax + k * n];
A[imax + k * n] = A[j + k * n];
A[j + k * n] = tmp;
}
*d = -(*d);
vv[imax] = vv[j];
}
indx[j] = imax;
if (A[j + j * n] == 0.0) {
free(vv);
return -1;
}
if (j != n - 1) {
tmp = 1.0 / A[j + j * n];
for (i = j + 1; i < n; i++)
A[i + j * n] *= tmp;
}
}
free(vv);
return 0;
}
11. LU 回代
static void lubksb(const double *A, int n, const int *indx, double *b) {
double s;
int i, ii = -1, ip, j;
for (i = 0; i < n; i++) {
ip = indx[i];
s = b[ip];
b[ip] = b[i];
if (ii >= 0)
for (j = ii; j < i; j++)
s -= A[i + j * n] * b[j];
else if (s)
ii = i;
b[i] = s;
}
for (i = n - 1; i >= 0; i--) {
s = b[i];
for (j = i + 1; j < n; j++)
s -= A[i + j * n] * b[j];
b[i] = s / A[i + i * n];
}
}
12. 矩阵的逆
extern int matinv(double *A, int n) {
double d, *B;
int i, j, *indx;
indx = imat(n, 1);
B = mat(n, n);
matcpy(B, A, n, n);
if (ludcmp(B, n, indx, &d)) {
free(indx);
free(B);
return -1;
}
for (j = 0; j < n; j++) {
for (i = 0; i < n; i++)
A[i + j * n] = 0.0;
A[j + j * n] = 1.0;
lubksb(B, n, indx, A + j * n);
}
free(indx);
free(B);
return 0;
}
13. 求解线性方程
extern int solve(const char *tr, const double *A, const double *Y, int n, int m, double *X) {
double *B = mat(n, n);
int info;
matcpy(B, A, n, n);
if (!(info = matinv(B, n)))
matmul(tr[0] == 'N' ? "NN" : "TN", n, m, n, 1.0, B, Y, 0.0, X);
free(B);
return info;
}
14. 最小二乘求最优解
/* least square estimation -----------------------------------------------------
* least square estimation by solving normal equation (x=(A*A')^-1*A*y)
* args : double *A I transpose of (weighted) design matrix (n x m)
* double *y I (weighted) measurements (m x 1)
* int n,m I number of parameters and measurements (n<=m)
* double *x O estimated parameters (n x 1)
* double *Q O estimated parameters covariance matrix (n x n)
* return : status (0:ok,0>:error)
* notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2))
* matrix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
extern int lsq(const double* A, const double* y, int n, int m, double* x, double* Q) {
double* Ay;
int info;
if (m < n) return -1;
Ay = mat(n, 1);
matmul("NN", n, 1, m, 1.0, A, y, 0.0, Ay); /* Ay=A*y */
matmul("NT", n, n, m, 1.0, A, A, 0.0, Q); /* Q=A*A' */
if (!(info = matinv(Q, n)))
matmul("NN", n, 1, n, 1.0, Q, Ay, 0.0, x); /* x=Q^-1*Ay */
free(Ay);
return info;
}
15. 卡尔曼滤波
/* kalman filter ---------------------------------------------------------------
* kalman filter state update as follows:
*
* K=P*H*(H'*P*H+R)^-1, xp=x+K*v, Pp=(I-K*H')*P
*
* args : double *x I states vector (n x 1)
* double *P I covariance matrix of states (n x n)
* double *H I transpose of design matrix (n x m)
* double *v I innovation (measurement - model) (m x 1)
* double *R I covariance matrix of measurement error (m x m)
* int n,m I number of states and measurements
* double *xp O states vector after update (n x 1)
* double *Pp O covariance matrix of states after update (n x n)
* return : status (0:ok,<0:error)
* notes : matrix stored by column-major order (fortran convention)
* if state x[i]==0.0, not updates state x[i]/P[i+i*n]
*-----------------------------------------------------------------------------*/
static int filter_(const double* x, const double* P, const double* H, const double* v, const double* R, int n, int m, double* xp, double* Pp) {
double* F = mat(n, m), * Q = mat(m, m), * K = mat(n, m), * I = eye(n);
int info;
matcpy(Q, R, m, m);
matcpy(xp, x, n, 1);
matmul("NN", n, m, n, 1.0, P, H, 0.0, F); /* Q=H'*P*H+R */
matmul("TN", m, m, n, 1.0, H, F, 1.0, Q);
if (!(info = matinv(Q, m))) {
matmul("NN", n, m, m, 1.0, F, Q, 0.0, K); /* K=P*H*Q^-1 */
matmul("NN", n, 1, m, 1.0, K, v, 1.0, xp); /* xp=x+K*v */
matmul("NT", n, n, m, -1.0, K, H, 1.0, I); /* Pp=(I-K*H')*P */
matmul("NN", n, n, n, 1.0, I, P, 0.0, Pp);
}
free(F);
free(Q);
free(K);
free(I);
return info;
}
extern int filter(double* x, double* P, const double* H, const double* v, const double* R, int n, int m) {
double* x_, * xp_, * P_, * Pp_, * H_;
int i, j, k, info, * ix;
ix = imat(n, 1);
for (i = k = 0; i < n; i++)
if (x[i] != 0.0 && P[i + i * n] > 0.0)
ix[k++] = i;
x_ = mat(k, 1);
xp_ = mat(k, 1);
P_ = mat(k, k);
Pp_ = mat(k, k);
H_ = mat(k, m);
for (i = 0; i < k; i++) {
x_[i] = x[ix[i]];
for (j = 0; j < k; j++)
P_[i + j * k] = P[ix[i] + ix[j] * n];
for (j = 0; j < m; j++)
H_[i + j * k] = H[ix[i] + j * n];
}
info = filter_(x_, P_, H_, v, R, k, m, xp_, Pp_);
for (i = 0; i < k; i++) {
x[ix[i]] = xp_[i];
for (j = 0; j < k; j++)
P[ix[i] + ix[j] * n] = Pp_[i + j * k];
}
free(ix);
free(x_);
free(xp_);
free(P_);
free(Pp_);
free(H_);
return info;
}
16. 平滑向前/向后滤波
/* smoother --------------------------------------------------------------------
* combine forward and backward filters by fixed-interval smoother as follows:
* 通过固定间隔平滑器组合前向和后向滤波器，如下所示：
*
* xs = Qs * (Qf^-1 * xf + Qb^-1 * xb)
* Qs = (Qf^-1 + Qb^-1)^-1
*
* args : double *xf I forward solutions (n x 1)
* double *Qf I forward solutions covariance matrix (n x n)
* double *xb I backward solutions (n x 1)
* double *Qb I backward solutions covariance matrix (n x n)
* int n I number of solutions
* double *xs O smoothed solutions (n x 1)
* double *Qs O smoothed solutions covariance matrix (n x n)
* return : status (0: ok, >0: error)
* notes : see reference [4] 5.2
* matrix stored by column-major order (Fortran convention)
*-----------------------------------------------------------------------------*/
extern int smoother(const double* xf, const double* Qf, const double* xb, const double* Qb, int n, double* xs, double* Qs) {
double* invQf = mat(n, n);
double* invQb = mat(n, n);
double* xx = mat(n, 1);
int i, info = -1;
matcpy(invQf, Qf, n, n);
matcpy(invQb, Qb, n, n);
if (!matinv(invQf, n) && !matinv(invQb, n)) {
for (i = 0; i < n * n; i++) {
Qs[i] = invQf[i] + invQb[i];
}
if (!(info = matinv(Qs, n))) {
matmul("NN", n, 1, n, 1.0, invQf, xf, 0.0, xx);
matmul("NN", n, 1, n, 1.0, invQb, xb, 1.0, xx);
matmul("NN", n, 1, n, 1.0, Qs, xx, 0.0, xs);
}
}
free(invQf);
free(invQb);
free(xx);
return info;
}
17. 矩阵输出
/* print matrix ----------------------------------------------------------------
* print matrix to stdout
* args : double *A I matrix A (n x m)
* int n, m I number of rows and columns of A
* int p, q I total columns, columns under decimal point
* FILE *fp I output file pointer
* return : none
* notes : matrix stored by column-major order (Fortran convention)
*-----------------------------------------------------------------------------*/
extern void matfprint(const double A[], int n, int m, int p, int q, FILE* fp) {
int i, j;
for (i = 0; i < n; i++) {
for (j = 0; j < m; j++) {
fprintf(fp, " %*.*f", p, q, A[i + j * n]);
}
fprintf(fp, "\n");
}
}
extern void matprint(const double A[], int n, int m, int p, int q) {
matfprint(A, n, m, p, q, stdout);
}
最新评论