Nu har jag skrivit två program. Ena programmet estimerar en matematisk modell av mätdata och det andra programmet räknar ut vilka insignaler man ska ha för att uppnå ett börvärde, dvs en referens.
Här kan ni se hur många matriser jag använder mig utav. Det är alltså många. Nu fattas det bara att jag kopplar ihop dessa två och sedan låter jag utsignalen U styra vilken PWM frekvens det ska vara.
Se bifogad fil. Ny uppdatering. Denna gång är ALLT double!
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;
}
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
*/
double A[row_a * column_a] = { 0, 1,
-3, -2};
double B[row_b * column_b] = { 0,
1
};
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 30
// 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 29 // 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/2, row_o * row_c, column_ry); // Ry*R = Ry (R need to be divided with 2)
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;
}
Då blir det Debian server. Tryggt och säker.