C语言实现矩阵的加减乘,转置,求逆等操作
2018-01-13 09:46
411 查看
说明
因算法仿真中涉及到了卡尔曼滤波器,而卡尔曼滤波器的预测方案的实现需要矩阵等诸多基本操作,现将常用操作整理如下:代码实现
/* Matrix math. */ #include <assert.h> #include <stdarg.h> #include <stdio.h> #include <stdlib.h> #include <math.h> #include "matrix.h" typedef struct { /* 维度 */ int rows; int cols; /* 内容 */ double** data; } Matrix; /* 为矩阵分配初始空间 */ Matrix alloc_matrix(int rows, int cols) { Matrix m; int i; int j; m.rows = rows; m.cols = cols; m.data = (double**) malloc(sizeof(double*) * m.rows); for ( i = 0; i < m.rows; ++i) { m.data[i] = (double*) malloc(sizeof(double) * m.cols); assert(m.data[i]); for ( j = 0; j < m.cols; ++j) { m.data[i][j] = 0.0; } } return m; } /* 释放空间 */ void free_matrix(Matrix m) { int i; assert(m.data != NULL); for ( i = 0; i < m.rows; ++i) { free(m.data[i]); } free(m.data); } /* 初始化矩阵 */ void set_matrix(Matrix m, ...) { va_list ap; int i,j; va_start(ap, m); for ( i = 0; i < m.rows; ++i) { for ( j = 0; j < m.cols; ++j) { m.data[i][j] = va_arg(ap, double); } } va_end(ap); } /* 转换为单元矩阵 */ void set_identity_matrix(Matrix m) { int i; int j; assert(m.rows == m.cols); for ( i = 0; i < m.rows; ++i) { for ( j = 0; j < m.cols; ++j) { if (i == j) { m.data[i][j] = 1.0; } else { m.data[i][j] = 0.0; } } } } /* 复制矩阵 */ void copy_matrix(Matrix source, Matrix destination) { int i; int j; assert(source.rows == destination.rows); assert(source.cols == destination.cols); for ( i = 0; i < source.rows; ++i) { for ( j = 0; j < source.cols; ++j) { destination.data[i][j] = source.data[i][j]; } } } /* 打印矩阵 */ void print_matrix(Matrix m) { int i; int j; for ( i = 0; i < m.rows; ++i) { for ( j = 0; j < m.cols; ++j) { if (j > 0) { printf(" "); } printf("%6.2f", m.data[i][j]); } printf("\n"); } } /* 矩阵相加 */ void add_matrix(Matrix a, Matrix b, Matrix c) { int i; int j; assert(a.rows == b.rows); assert(a.rows == c.rows); assert(a.cols == b.cols); assert(a.cols == c.cols); for ( i = 0; i < a.rows; ++i) { for ( j = 0; j < a.cols; ++j) { c.data[i][j] = a.data[i][j] + b.data[i][j]; } } } /* 矩阵相减 */ void subtract_matrix(Matrix a, Matrix b, Matrix c) { int i; int j; assert(a.rows == b.rows); assert(a.rows == c.rows); assert(a.cols == b.cols); assert(a.cols == c.cols); for ( i = 0; i < a.rows; ++i) { for ( j = 0; j < a.cols; ++j) { c.data[i][j] = a.data[i][j] - b.data[i][j]; } } } /* 用单元矩阵减去该矩阵 */ void subtract_from_identity_matrix(Matrix a) { int i; int j; assert(a.rows == a.cols); for ( i = 0; i < a.rows; ++i) { for ( j = 0; j < a.cols; ++j) { if (i == j) { a.data[i][j] = 1.0 - a.data[i][j]; } else { a.data[i][j] = 0.0 - a.data[i][j]; } } } } /* 矩阵相乘 */ void multiply_matrix(Matrix a, Matrix b, Matrix c) { int i; int j; int k; assert(a.cols == b.rows); assert(a.rows == c.rows); assert(b.cols == c.cols); for ( i = 0; i < c.rows; ++i) { for ( j = 0; j < c.cols; ++j) { /* Calculate element c.data[i][j] via a dot product of one row of a with one column of b */ c.data[i][j] = 0.0; for ( k = 0; k < a.cols; ++k) { c.data[i][j] += a.data[i][k] * b.data[k][j]; } } } } /* 乘以一个矩阵的转置矩阵. */ void multiply_by_transpose_matrix(Matrix a, Matrix b, Matrix c) { int i; int j; int k; assert(a.cols == b.cols); assert(a.rows == c.rows); assert(b.rows == c.cols); for ( i = 0; i < c.rows; ++i) { for ( j = 0; j < c.cols; ++j) { /* Calculate element c.data[i][j] via a dot product of one row of a with one row of b */ c.data[i][j] = 0.0; for ( k = 0; k < a.cols; ++k) { c.data[i][j] += a.data[i][k] * b.data[j][k]; } } } } /* 矩阵转置 */ void transpose_matrix(Matrix input, Matrix output) { int i; int j; //int k; assert(input.rows == output.cols); assert(input.cols == output.rows); for ( i = 0; i < input.rows; ++i) { for ( j = 0; j < input.cols; ++j) { output.data[j][i] = input.data[i][j]; } } } /* 两矩阵是否相等 */ int equal_matrix(Matrix a, Matrix b, double tolerance) { int i; int j; //int k; assert(a.rows == b.rows); assert(a.cols == b.cols); for ( i = 0; i < a.rows; ++i) { for ( j = 0; j < a.cols; ++j) { if (abs(a.data[i][j] - b.data[i][j]) > tolerance) { return 0; } } } return 1; } /* 矩阵乘以一个系数 */ void scale_matrix(Matrix m, double scalar) { int i; int j; assert(scalar != 0.0); for ( i = 0; i < m.rows; ++i) { for ( j = 0; j < m.cols; ++j) { m.data[i][j] *= scalar; } } } /* 交换矩阵的两行 */ void swap_rows(Matrix m, int r1, int r2) { double *tmp; assert(r1 != r2); tmp = m.data[r1]; m.data[r1] = m.data[r2]; m.data[r2] = tmp; } /* 矩阵某行乘以一个系数 */ void scale_row(Matrix m, int r, double scalar) { int i; assert(scalar != 0.0); for ( i = 0; i < m.cols; ++i) { m.data[r][i] *= scalar; } } /* Add scalar * row r2 to row r1. */ void shear_row(Matrix m, int r1, int r2, double scalar) { int i; assert(r1 != r2); for ( i = 0; i < m.cols; ++i) { m.data[r1][i] += scalar * m.data[r2][i]; } } /* 矩阵的求逆(借鉴他人) */ /* Uses Gauss-Jordan elimination. The elimination procedure works by applying elementary row operations to our input matrix until the input matrix is reduced to the identity matrix. Simultaneously, we apply the same elementary row operations to a separate identity matrix to produce the inverse matrix. If this makes no sense, read wikipedia on Gauss-Jordan elimination. This is not the fastest way to invert matrices, so this is quite possibly the bottleneck. */ int destructive_invert_matrix(Matrix input, Matrix output) { int i; int j; int r; double scalar; double shear_needed; assert(input.rows == input.cols); assert(input.rows == output.rows); assert(input.rows == output.cols); set_identity_matrix(output); /* Convert input to the identity matrix via elementary row operations. The ith pass through this loop turns the element at i,i to a 1 and turns all other elements in column i to a 0. */ for ( i = 0; i < input.rows; ++i) { if (input.data[i][i] == 0.0) { /* We must swap rows to get a nonzero diagonal element. */ for (r = i + 1; r < input.rows; ++r) { if (input.data[r][i] != 0.0) { break; } } if (r == input.rows) { /* Every remaining element in this column is zero, so this matrix cannot be inverted. */ return 0; } swap_rows(input, i, r); swap_rows(output, i, r); } /* Scale this row to ensure a 1 along the diagonal. We might need to worry about overflow from a huge scalar here. */ scalar = 1.0 / input.data[i][i]; scale_row(input, i, scalar); scale_row(output, i, scalar); /* Zero out the other elements in this column. */ for ( j = 0; j < input.rows; ++j) { if (i == j) { continue; } shear_needed = -input.data[j][i]; shear_row(input, j, i, shear_needed); shear_row(output, j, i, shear_needed); } } return 1; } /* 卡尔曼预测 */ void predict(Matrix* x_priori, Matrix* p_priori, Matrix Z, double timeStep) { Matrix A; Matrix H; Matrix Q; Matrix R; Matrix K; Matrix x_posteriori; Matrix p_posteriori; Matrix temp1; Matrix temp2; Matrix temp3; Matrix temp4; Matrix temp5; Matrix temp6; Matrix temp7; Matrix temp8; Matrix temp9; Matrix temp10; Matrix temp11; Matrix temp12; Matrix temp13; Matrix temp14; Matrix temp15; Matrix temp16; A = alloc_matrix(3,3); H = alloc_matrix(2,3); Q = alloc_matrix(3,3); R = alloc_matrix(2,2); K = alloc_matrix(3,2); x_posteriori = alloc_matrix(3,1); p_posteriori = alloc_matrix(3,3); temp1 = alloc_matrix(2,3); temp2 = alloc_matrix(3,2); temp3 = alloc_matrix(2,2); temp4 = alloc_matrix(2,2); temp5 = alloc_matrix(2,2); temp6 = alloc_matrix(3,2); temp7 = alloc_matrix(2,1); temp8 = alloc_matrix(2,1); temp9 = alloc_matrix(3,1); temp10 = alloc_matrix(3,3); temp11 = alloc_matrix(3,3); temp12 = alloc_matrix(3,1); temp13 = alloc_matrix(3,3); temp14 = alloc_matrix(3,3); temp15 = alloc_matrix(3,3); temp16 = alloc_matrix(3,3); set_matrix(A, 1.0, 0.0, timeStep, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); set_matrix(H, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0); set_matrix(Q, 0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001); set_matrix(R, 1.0, 0.0, 0.0, 1.0); multiply_matrix(H, *p_priori, temp1); transpose_matrix(H, temp2); multiply_matrix(temp1, temp2, temp3); add_matrix(temp3, R, temp4); destructive_invert_matrix(temp4, temp5); multiply_matrix(*p_priori, temp2, temp6); multiply_matrix(temp5, temp6, K); multiply_matrix(H, *x_priori, temp7); subtract_matrix(Z, temp7, temp8); multiply_matrix(K, temp8, temp9); add_matrix(*x_priori, temp9, x_posteriori); multiply_matrix(K, H, temp10); subtract_from_identity_matrix(temp10); multiply_matrix(temp10, *p_priori, temp11); multiply_matrix(A, x_posteriori, temp12); copy_matrix(temp12, *x_priori); multiply_matrix(A, p_posteriori, temp13); transpose_matrix(A, temp14); multiply_matrix(temp13, temp14, temp15); add_matrix(temp15, Q, temp16); copy_matrix(temp16, *p_priori); free_matrix(temp1); free_matrix(temp2); free_matrix(temp3); free_matrix(temp4); free_matrix(temp5); free_matrix(temp6); free_matrix(temp7); free_matrix(temp8); free_matrix(temp9); free_matrix(temp10); free_matrix(temp11); free_matrix(temp12); free_matrix(temp13); free_matrix(temp14); free_matrix(temp15); free_matrix(temp16); }
相关文章推荐
- 矩阵的转置和行列式等操作的c语言实现
- c语言实现矩阵的三元组表示 + 矩阵的转置
- C语言学习之用二维数组实现矩阵转置
- PHP简单实现二维数组的矩阵转置操作示例
- MMX实现的矩阵转置操作
- C语言 动态生成矩阵,并实现其录入转置
- 数据结构之---C语言实现矩阵的转置
- Linux下的C语言编程——指针实现矩阵转置
- 矩阵基本操作(加减乘、求逆、转置)
- Java实现的矩阵类及矩阵的转置,加减乘和矩阵求逆
- 稀疏矩阵的存储以及转置、加法、乘法操作实现
- C语言实现矩阵的转置
- Java进阶(四十九)实现矩阵秩的求解-转置-行列式-逆矩阵操作
- 数据结构--单链表的基本操作(C语言实现)
- C语言实现的矩阵库meschach
- 矩阵的掩膜操作实现图像对比度调整
- C语言字符串的常见特殊操作(除了string.c实现的那些接口)
- 学习笔记——线性表的动态分配顺序存储结构基本操作(C语言实现)
- c语言自定义方法实现字符串的7个操作
- c语言字符串操作实现