我需要将此代码转换为C代码。
问题：
foo（float *输入，float *输出）
function [U] = mpc (A, B, C, x, N, r, lb)
## Find matrix
PHI = phiMat(A, C, N);
GAMMA = gammaMat(A, B, C, N);
## Solve first with no constraints
U = solve(PHI, GAMMA, x, N, r, 0, 0, false);
## Then use the last U as upper bound
U = solve(PHI, GAMMA, x, N, r, lb, U(end), true);
end
function U = solve(PHI, GAMMA, x, N, r, lb, ub, constraints)
## Set U
U = zeros(N, 1);
## Iterate Gaussian Elimination
for i = 1:N
## Solve u
if(i == 1)
u = (r - PHI(i,:)*x)/GAMMA(i,i)
else
u = (r - PHI(i,:)*x - GAMMA(i,1:i-1)*U(1:i-1) )/GAMMA(i,i)
end
## Constraints
if(constraints == true)
if(u > ub)
u = ub;
elseif(u < lb)
u = lb;
end
end
## Save u
U(i) = u
end
end
function PHI = phiMat(A, C, N)
## Create the special Observabillity matrix
PHI = [];
for i = 1:N
PHI = vertcat(PHI, C*A^i);
end
end
function GAMMA = gammaMat(A, B, C, N)
## Create the lower triangular toeplitz matrix
GAMMA = [];
for i = 1:N
GAMMA = horzcat(GAMMA, vertcat(zeros((i-1)*size(C*A*B, 1), size(C*A*B, 2)),cabMat(A, B, C, N-i+1)));
end
end
function CAB = cabMat(A, B, C, N)
## Create the column for the GAMMA matrix
CAB = [];
for i = 0:N-1
CAB = vertcat(CAB, C*A^i*B);
end
end
我的C代码。是的，它的工作原理！
/* * Generalized_Predictive_Control.c * * Created on: * Author: */ #include "Generalized_Predictive_Control.h" /* * Parameters */ int adim; int ydim; int rdim; int horizon; /* * Deceleration */ static void obsv(float* PHI, const float* A, const float* C); static void kalman(float* x, const float* A, const float* B, float* u, const float* K, float* y, const float* C); static void mul(float* A, float* B, float* C, int row_a, int column_a, int column_b); static void tran(float* A, int row, int column); static void CAB(float* GAMMA, float* PHI, const float* A, const float* B, const float* C); static void solve(float* GAMMA, float* PHI, float* x, float* u, float* r, float lb, float ub, int constraintsON); static void print(float* A, int row, int column); void GPC(int adim_, int ydim_, int rdim_, int horizon_, const float* A, const float* B, const float* C, const float* D, const float* K, float* u, float* r, float* y, float* x){ /* * Set the dimensions */ adim = adim_; ydim = ydim_; rdim = rdim_; horizon = horizon_; /* * Identify the model - Extended Least Square */ int n = 5; float* phi; float* theta; //els(phi, theta, n, y, u, P); /* * Create a state space model with Observable canonical form */ /* * Create the extended observability matrix */ float PHI[horizon*ydim*adim]; memset(PHI, 0, horizon*ydim*adim*sizeof(float)); obsv(PHI, A, C); /* * Create the lower triangular toeplitz matrix */ float GAMMA[horizon*rdim*horizon*ydim]; memset(GAMMA, 0, horizon*rdim*horizon*ydim*sizeof(float)); CAB(GAMMA, PHI, A, B, C); /* * Solve the best input value */ solve(GAMMA, PHI, x, u, r, 0, 0, 0); solve(GAMMA, PHI, x, u, r, 0, *(u), 1); /* * Estimate the state vector */ kalman(x, A, B, u, K, y, C); } /* * Identify the model */ static void els(float* P, float* phi, float* theta, int polyLength, int totalPolyLength, float* y, float* u, float* e){ /* * move phi with the inputs, outputs, errors one step to right */ for(int i = 0; i < polyLength; i++){ *(phi + i+1 + totalPolyLength*0) = *(phi + i + totalPolyLength*0); // Move one to right for the y's *(phi + i+1 + totalPolyLength*1) = *(phi + i + totalPolyLength*1); // Move one to right for the u's *(phi + i+1 + totalPolyLength*2) = *(phi + i + totalPolyLength*2); // Move one to right for the e's } /* * Add the current y, u and e (*phi + totalPolyLength*0) = -*(y + 0); // Need to be negative! (*phi + totalPolyLength*1) = *(u + 0); (*phi + totalPolyLength*2) = *(e + 0); */ /* * phi'*theta */ float y_est = 0; for(int i = 0; i < totalPolyLength; i++){ y_est += *(phi + i) * *(theta + i); } float epsilon = *(y + 0) - y_est; // In this case, y is only one element array /* * phi*epsilon */ float phi_epsilon[totalPolyLength]; memset(phi_epsilon, 0, totalPolyLength*sizeof(float)); for(int i = 0; i < totalPolyLength; i++){ *(phi_epsilon + i) = *(phi + i) * epsilon; } /* * P_vec = P*phi_epsilon */ float P_vec[totalPolyLength]; memset(P_vec, 0, totalPolyLength*sizeof(float)); mul(P, phi_epsilon, P_vec, totalPolyLength, totalPolyLength, 1); /* * Update our estimated vector theta = theta + P_vec */ for(int i = 0; i < totalPolyLength; i++){ *(theta + i) = *(theta + i) + *(P_vec + i); } /* * Update P = P - (P*phi*phi'*P)/(1 + phi'*P*phi) */ // Create phi' float phiT[totalPolyLength]; memset(phiT, 0, totalPolyLength*sizeof(float)); memcpy(phiT, phi, totalPolyLength*sizeof(float)); tran(phiT, totalPolyLength, 1); // phi'*P float phiT_P[totalPolyLength]; memset(phiT_P, 0, totalPolyLength*sizeof(float)); mul(phiT, P, phiT_P, 1, totalPolyLength, totalPolyLength); // phi*phi'*P float phi_phiT_P[totalPolyLength*totalPolyLength]; memset(phi_phiT_P, 0, totalPolyLength*totalPolyLength*sizeof(float)); mul(phi, phiT_P, phi_phiT_P, totalPolyLength, 1, totalPolyLength); // P*phi*phi'*P float P_phi_phiT_P[totalPolyLength*totalPolyLength]; memset(P_phi_phiT_P, 0, totalPolyLength*totalPolyLength*sizeof(float)); mul(P, phi_phiT_P, P_phi_phiT_P, totalPolyLength, totalPolyLength, totalPolyLength); // P*phi float P_phi[totalPolyLength]; memset(P_phi, 0, totalPolyLength*sizeof(float)); mul(P, phi, P_phi, totalPolyLength, totalPolyLength, 1); // phi'*P*phi float phiT_P_phi[1]; memset(phiT_P_phi, 0, 1*sizeof(float)); mul(phiT, P_phi, phiT_P_phi, 1, totalPolyLength, 1); // P = P - (P_phi_phiT_P) / (1+phi'*P*phi) for(int i = 0; i < totalPolyLength*totalPolyLength; i++){ *(P + i) = *(P + i) - *(P_phi_phiT_P + i) / (1 + *(phiT_P_phi)); } } /* * This will solve if GAMMA is square! */ static void solve(float* GAMMA, float* PHI, float* x, float* u, float* r, float lb, float ub, int constraintsON){ /* * Now we are going to solve on the form * Ax=b, where b = (R*r-PHI*x) and A = GAMMA and x = U */ /* * R_vec = R*r */ float R_vec[horizon*ydim]; memset(R_vec, 0, horizon*ydim*sizeof(float)); for(int i = 0; i < horizon*ydim; i++){ for (int j = 0; j < rdim; j++) { *(R_vec + i + j) = *(r + j); } i += rdim-1; } /* * PHI_vec = PHI*x */ float PHI_vec[horizon*ydim]; memset(PHI_vec, 0, horizon * ydim * sizeof(float)); mul(PHI, x, PHI_vec, horizon*ydim, adim, 1); /* * Solve now (R_vec - PHI_vec) = GAMMA*U * Notice that this is ONLY for Square GAMMA with lower triangular toeplitz matrix e.g SISO case * This using Gaussian Elimination backward substitution */ float U[horizon]; float sum = 0.0; memset(U, 0, horizon*sizeof(float)); for(int i = 0; i < horizon; i++){ for(int j = 0; j < i; j++){ sum += *(GAMMA + i*horizon + j) * *(U + j); } float newU = (*(R_vec + i) - *(PHI_vec + i) - sum) / (*(GAMMA + i*horizon + i)); if(constraintsON == 1){ if(newU > ub) newU = ub; if(newU < lb) newU = lb; } *(U + i) = newU; sum = 0.0; } //print(U, horizon, 1); /* * Set last U to u */ if(constraintsON == 0){ *(u + 0) = *(U + horizon - 1); }else{ *(u + 0) = *(U + 0); } } /* * Lower traingular toeplitz of extended observability matrix */ static void CAB(float* GAMMA, float* PHI, const float* A, const float* B, const float* C){ /* * First create the initial C*A^0*B == C*I*B == C*B */ float CB[ydim*rdim]; memset(CB, 0, ydim*rdim*sizeof(float)); mul((float*)C, (float*)B, CB, ydim, adim, rdim); /* * Take the transpose of CB so it will have dimension rdim*ydim instead */ tran(CB, ydim, rdim); /* * Create the CAB matrix from PHI*B */ float PHIB[horizon*ydim*rdim]; mul(PHI, (float*) B, PHIB, horizon*ydim, adim, rdim); // CAB = PHI*B tran(PHIB, horizon*ydim, rdim); /* * We insert GAMMA = [CB PHI; * 0 CB PHI; * 0 0 CB PHI; * 0 0 0 CB PHI] from left to right */ for(int i = 0; i < horizon; i++) { for(int j = 0; j < rdim; j++) { memcpy(GAMMA + horizon*ydim*(i*rdim+j) + ydim*i, CB + ydim*j, ydim*sizeof(float)); // Add CB memcpy(GAMMA + horizon*ydim*(i*rdim+j) + ydim*i + ydim, PHIB + horizon*ydim*j, (horizon-i-1)*ydim*sizeof(float)); // Add PHI*B } } /* * Transpose of gamma */ tran(GAMMA, horizon*rdim, horizon*ydim); //print(CB, rdim, ydim); //print(PHIB, rdim, horizon*ydim); //print(GAMMA, horizon*ydim, horizon*rdim); } /* * Transpose */ static void tran(float* A, int row, int column) { float B[row*column]; float* transpose; float* ptr_A = A; for (int i = 0; i < row; i++) { transpose = &B[i]; for (int j = 0; j < column; j++) { *transpose = *ptr_A; ptr_A++; transpose += row; } } // Copy! memcpy(A, B, row*column*sizeof(float)); } /* * [C*A^1; C*A^2; C*A^3; ... ; C*A^horizon] % Extended observability matrix */ static void obsv(float* PHI, const float* A, const float* C){ /* * This matrix will A^(i+1) all the time */ float A_pow[adim*adim]; memset(A_pow, 0, adim * adim * sizeof(float)); float A_copy[adim*adim]; memcpy(A_copy, (float*) A, adim * adim * sizeof(float)); /* * Temporary matrix */ float T[ydim*adim]; memset(T, 0, ydim * adim * sizeof(float)); /* * Regular T = C*A^(1+i) */ mul((float*) C, (float*) A, T, ydim, adim, adim); /* * Insert temporary T into PHI */ memcpy(PHI, T, ydim*adim*sizeof(float)); /* * Do the rest C*A^(i+1) because we have already done i = 0 */ for(int i = 1; i < horizon; i++){ mul((float*) A, A_copy, A_pow, adim, adim, adim); // Matrix power A_pow = A*A_copy mul((float*) C, A_pow, T, ydim, adim, adim); // T = C*A^(1+i) memcpy(PHI + i*ydim*adim, T, ydim*adim*sizeof(float)); // Insert temporary T into PHI memcpy(A_copy, A_pow, adim * adim * sizeof(float)); // A_copy <- A_pow } } /* * x = Ax - KCx + Bu + Ky % Kalman filter */ static void kalman(float* x, const float* A, const float* B, float* u, const float* K, float* y, const float* C) { /* * Compute the vector A_vec = A*x */ float A_vec[adim*1]; memset(A_vec, 0, adim*sizeof(float)); mul((float*) A, x, A_vec, adim, adim, 1); /* * Compute the vector B_vec = B*u */ float B_vec[adim*1]; memset(B_vec, 0, adim*sizeof(float)); mul((float*) B, u, B_vec, adim, rdim, 1); /* * Compute the vector C_vec = C*x */ float C_vec[ydim*1]; memset(C_vec, 0, ydim*sizeof(float)); mul((float*) C, x, C_vec, ydim, adim, 1); /* * Compute the vector KC_vec = K*C_vec */ float KC_vec[adim*1]; memset(KC_vec, 0, adim*sizeof(float)); mul((float*) K, C_vec, KC_vec, adim, ydim, 1); /* * Compute the vector Ky_vec = K*y */ float Ky_vec[adim*1]; memset(Ky_vec, 0, adim*sizeof(float)); mul((float*) K, y, Ky_vec, adim, ydim, 1); /* * Now add x = A_vec - KC_vec + B_vec + Ky_vec */ for(int i = 0; i < adim; i++){ *(x + i) = *(A_vec + i) - *(KC_vec + i) + *(B_vec + i) + *(Ky_vec + i); } } /* * C = A*B */ static void mul(float* A, float* B, float* C, int row_a, int column_a, int column_b) { // Data matrix float* data_a = A; float* data_b = B; for (int i = 0; i < row_a; i++) { // Then we go through every column of b for (int j = 0; j < column_b; j++) { data_a = &A[i * column_a]; data_b = &B[j]; *C = 0; // Reset // And we multiply rows from a with columns of b for (int k = 0; k < column_a; k++) { *C += *data_a * *data_b; data_a++; data_b += column_b; } C++; // ;) } } } /* * Print matrix or vector - Just for error check */ static void print(float* A, int row, int column) { for (int i = 0; i < row; i++) { for (int j = 0; j < column; j++) { printf("%0.18f ", *(A++)); } printf("\n"); } printf("\n"); }
免责声明：我在MATLAB Coder上工作
cfg = coder.config('lib');
cfg.DynamicMemoryAllocation = 'Off';
codegen -config cfg ...
foo(x_data[100], x_size[2])horzcat和vertcat之类的基本运算产生独立于MATLAB的可移植代码。function A = foo(A,B)的调用如下：y = foo(y,z);可以产生类似于void foo(double A[100], const double B[20]);的东西，其中A是输入和输出。