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;
}