Re: Matrisberäkningar med för STM32?
Postat: 24 februari 2019, 23:38:10
				
				Testa det här: http://abyz.me.uk/rpi/pigpio/index.html
			Svenskt forum för elektroniksnack.
https://elektronikforumet.com/forum/
Kod: Markera allt
#include <time.h>
#include "LinearAlgebra/declareFunctions.h"
/*
 * Here you can set the sizes for the matrices
 */
#define row_a 2 // A row
#define column_a 2 // A column
#define row_b 2 // B row, the same row as A.
#define column_b 1 // B column, the same column as D
#define row_c 1 // C row, the same row as D
#define column_c 2 // C column, the same column as A
/*
 * Create a state space model - discrete
 */
double A[row_a * column_a] = { -0.099272, 0.029481,
		-0.088444, -0.158234};
double B[row_b * column_b] = { 0.366424,
							   0.029481
};
double C[row_c * column_c] = {1, 0};
double D[row_c * column_b] = {0};
double X[row_a] = {0, 0};
double R =  6;
// Create the length of the observability matrix, notice it will have the dimension (row_o * row_c + row_c) x column_b
#define row_o 20
// Create the widt of the lower triangular toeplitz H matrix, notice that it will have the dimension (row_o * row_c + row_c) x (column_h * column_b)
#define column_h 19 // column_h < row_o - always!
// Define the column of the reference vector - Standard is 1
#define column_ry 1
int main() {
	/*
	 * Model predictive control
	 */
	clock_t start, end;
	float cpu_time_used;
	start = clock();
	/*
	 * Create the Observabillity matrix and the
	 */
	double O[(row_o * row_c) * row_a];
	double O_[(row_o * row_c) * row_a]; // This is for the lower triangular toeplitz matrix
	double A_[row_a * column_a];
	double C_[row_c * column_c];
	for (int i = 1; i <= row_o; i++) {
		copy(A, A_, row_a, column_a); // Copy A -> A_
		mpower(A_, row_a, i); // Power A_^i
		mul(C, A_, false, C_, row_c, column_c, column_a); // C_ = C*A_
		insert(C_, O, row_c, column_c, row_a, (i-1)*row_c, 0); // Insert O = [CA^1; CA^2; CA^3; ... ; CA^row_o];
		copy(A, A_, row_a, column_a); // Copy A -> A_
		mpower(A_, row_a, i - 1); // Power A_^(i-1)
		mul(C, A_, false, C_, row_c, column_c, column_a); // C_ = C*A_
		insert(C_, O_, row_c, column_c, row_a, (i-1)*row_c, 0); // Insert O_ = [CA^0; CA^1; CA^2; ... ; CA^(row_o-1)];
	}
	//print(O, row_o * row_c, row_a);
	/*
	 * Create the lower triangular toeplitz matrix
	 */
	double H[(row_o * row_c) * (column_h * column_b)];
    zeros(H, row_o * row_c, column_h * column_b);
	// T = O_*B - Observabillity matrix times B
    double T[(row_o * row_c) * column_b];
	mul(O_, B, false, T, row_o * row_c, row_a, column_b);
	// Lower triangular toeplitz matrix of T = [C*A^0*B; C*A^1*B; C*A^2*B; C*A^3*B; ...; C*A^(row_o-1)*B]
	for (int j = 0; j < column_h; j++){
		insert(T, H, row_o * row_c, column_b, column_h * column_b, 0, j*column_b);
		move(T, row_o * row_c, column_b, row_c , 0);
	}
	//print(H, row_o * row_c, column_h * column_b); // H matrix
	/*
	 * Compute U = pinv(H)*(Ry*R - O*X), where R is our reference vector, X is our initial state vector
	 */
	pinv(H, row_o * row_c, column_h * column_b); // Pseudo inverse of H. Using the SVD method
	double Ry[(row_o * row_c)*column_ry];
	ones(Ry, row_o * row_c, column_ry);
	scale(Ry, R, row_o * row_c, column_ry); // Ry*R = Ry
	double OX[(row_o * row_c)*column_ry];
	mul(O, X, false, OX, row_o * row_c, row_a, column_ry); // O*X
	double Ry_OX[(row_o * row_c)*column_ry];
	sub(Ry, OX, Ry_OX, row_o * row_c, column_ry, column_ry); // Ry-O*X
	double U[(column_h * column_b)*column_ry];
	mul(H, Ry_OX, false, U, column_h * column_b, row_o * row_c, column_ry); // U = pinv(H)*(Ry-O*X);
	/*
	 * Our best input values
	 */
	print(U, column_h * column_b, column_ry);
	end = clock();
	cpu_time_used = ((float) (end - start)) / CLOCKS_PER_SEC;
	printf("\nTotal speed  was %f,", cpu_time_used);
	return 0;
}
Kod: Markera allt
#include <time.h>
#include "LinearAlgebra/declareFunctions.h"
int main() {
	/*
	 * G(s) = 1/(s^2 + 1s + 3)  - Model
	 * y = measured output values
	 * u = measured input values
	 */
	clock_t start, end;
	float cpu_time_used;
	start = clock();
	double y[144] = { 0.00000, 0.49525, 1.43863, 2.13779, 2.30516, 2.05713,
			1.69220, 1.45608, 1.42777, 1.54146, 1.67927, 1.75624, 1.75400,
			1.70478, 1.65394, 1.62996, 1.63549, 1.65594, 1.67426, 1.68125,
			1.67752, 1.66930, 1.66285, 1.66102, 1.66300, 1.66621, 1.66842,
			1.66880, 1.66786, 1.66664, 1.66591, 1.66588, 1.66629, 1.66675,
			1.66698, 1.66695, 1.66678, 1.66661, 1.66654, 1.66657, 1.66664,
			1.66670, 1.66672, 1.66670, 1.66667, 1.66665, 1.66665, 1.66666,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66666, 1.66666,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667, 1.66667,
			1.66667, 1.66667, 1.66667, 1.66667, 1.66667 };
	double u[144] = { 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
			5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
			5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
			5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
			5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
			5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
			5, 5, 5, 5, 5, 5, 5, 5, 5 };
	// Toeplitz matrix
	double toe[144 * 144];
	toeplitz(u, toe, 144);
	// Upper triangular
	double tru[144 * 144];
	triu(toe, tru, 0, 144, 144);
	// inverse
	inv(tru, 144);
	// Multiplication
	double g[144];
	mul(y, tru, false, g, 1, 144, 144);
	// Create hankel
	double H0[144 * 144];
	hankel(g, H0, 144, 1);
	double H1[144 * 144];
	hankel(g, H1, 144, 2);
	// Cut hankel into half
	double H0_half[72 * 72];
	double H1_half[72 * 72];
	cut(H0, 144, 144, H0_half, 0, 71, 0, 71);
	cut(H1, 144, 144, H1_half, 0, 71, 0, 71);
	// Do SVD
	double U[72 * 72];
	double S[72 * 72];
	double V[72 * 72];
	svd(H0_half, U, S, V, 72, 72);
	// Model reduction to second order model
	double Un[72 * 2];
	double Sn[2 * 2];
	double Vn[72 * 2];
	cut(U, 72, 72, Un, 0, 71, 0, 1);
	cut(S, 72, 72, Sn, 0, 1, 0, 1);
	cut(V, 72, 72, Vn, 0, 71, 0, 1);
	// Create A, B, C
	double Sn_sqrt_negative[2 * 2];
	double Sn_sqrt_positive[2 * 2];
	copy(Sn, Sn_sqrt_negative, 2,2);
	copy(Sn, Sn_sqrt_positive, 2,2);
	diagpower(Sn_sqrt_negative, -0.5, 2, 2);
	diagpower(Sn_sqrt_positive, 0.5, 2, 2);
	/*
	 * C = Un*Sn^(1/2);
	 * Cd = C(1, 1:2)
	 */
	double C[72 * 2];
	double Cd[1 * 2];
	mul(Un, Sn_sqrt_positive, false, C, 72, 2, 2);
	cut(C, 72, 2, Cd, 0, 0, 0, 1);
	/*
	 * Ad = Sn^(-1/2)*Un'*H1*Vn*Sn^(-1/2);
	 */
	double A[72 * 2];
	double A1[72 * 2];
	double Ad[2 * 2];
	double Ad1[2 * 2];
	mul(Vn, Sn_sqrt_negative, false, A, 72, 2, 2);
	mul(H1_half, A, false, A1, 72, 72, 2);
	tran(Un, 72, 2);
	mul(Un, A1, false, Ad1, 2, 72, 2);
	mul(Sn_sqrt_negative, Ad1, false, Ad, 2, 2, 2);
	/*
	 * B = Sn^(1/2)*Vn'
	 * Bd = B(:, 1);
	 */
	double B[2 * 72];
	double Bd[2 * 1];
	tran(Vn, 72, 2);
	mul(Sn_sqrt_positive, Vn, false, B, 2, 2, 72);
	cut(B, 2, 72, Bd, 0, 1, 0, 0);
	/*
	 * Print A, B, C
	 */
	printf("A Matrix: \n");
	print(Ad, 2, 2);
	printf("B Matrix: \n");
	print(Bd, 2, 1);
	printf("C Matrix: \n");
	print(Cd, 1, 2);
	end = clock();
	cpu_time_used = ((float) (end - start)) / CLOCKS_PER_SEC;
	printf("\nTotal speed  was %f,", cpu_time_used);
	return 0;
}