Nu har jag "optimerat" min matriskod. Nu åternavänder jag matriserna. För mig är det en enda röra.
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();
	float 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 };
	float 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
	float toe[144 * 144];
	toeplitz(u, toe, 144);
	// Upper triangular
	float tru[144 * 144];
	triu(toe, tru, 0, 144, 144);
	// inverse
	inv(tru, 144);
	// Multiplication
	//float u[144]; // Ersatt g med u
	mul(y, tru, false, u, 1, 144, 144);
	// Create hankel
	float H0[144 * 144];
	hankel(u, H0, 144, 1);
	// Cut hankel into half
	float H0_half[72 * 72];
	cut(H0, 144, 144, H0_half, 0, 71, 0, 71);
	// Do SVD
	float U[72 * 72];
	float S[72 * 72];
	float V[72 * 72];
	svd(H0_half, U, S, V, 72, 72);
	//float H0[144 * 144]; // Ersat H1 med H0
	//float H0_half[72 * 72]; // Ersatt H1_half med H0_half
	hankel(u, H0, 144, 2);
	cut(H0, 144, 144, H0_half, 0, 71, 0, 71);
	// Model reduction to second order model
	//float u[72 * 2]; // Ersatt Un med u
	float Ad[2 * 2];
	//float y[72 * 2]; // Ersatt Vn med y
	cut(U, 72, 72, u, 0, 71, 0, 1);
	cut(S, 72, 72, Ad, 0, 1, 0, 1);
	cut(V, 72, 72, y, 0, 71, 0, 1);
	// Create A, B, C
	float Sn_sqrt_negative[2 * 2]; // -- Sn_sqrt_negative kan vara exakt samma som Sn_sqrt_positive
	float Sn_sqrt_positive[2 * 2];
	copy(Ad, Sn_sqrt_negative, 2,2);
	copy(Ad, Sn_sqrt_positive, 2,2);
	powdiag(Sn_sqrt_negative, -0.5, 2, 2);
	powdiag(Sn_sqrt_positive, 0.5, 2, 2);
	/*
	 * C = Un*En^(1/2);
	 * Cd = C(1, 1:2)
	 */
	float C[72 * 2];
	float Cd[1 * 2];
	mul(u, 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);
	 */
	//float C[72 * 2]; // Ersatt A med C
	//float A1[72 * 2];  // Ersatt A1 med y
	//float Sn[2 * 2]; // Ersatt Ad med Sn
	float Ad1[2 * 2];
	mul(y, Sn_sqrt_negative, false, C, 72, 2, 2);
	mul(H0_half, C, false, y, 72, 72, 2);
	tran(u, 72, 2);
	mul(u, y, false, Ad1, 2, 72, 2);
	mul(Sn_sqrt_negative, Ad1, false, Ad, 2, 2, 2);
	/*
	 * B = En^(1/2)*Vn'
	 * Bd = B(:, 1);
	 */
	//float C[2 * 72]; // A ersatt med C
	float Bd[2 * 1];
	tran(y, 72, 2);
	mul(Sn_sqrt_positive, y, false, C, 2, 2, 72);
	cut(C, 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;
}