\(P = A^\top P A -(A^\top P B)(R + B^\top P B)^{-1}(B^\top P A) + Q.\)
Jag tänkte att jag ger er .c kod och ett arbetsexempel till att börja med. Detta exempel löser alltså ett massa-dämpare-fjäder system med hjälp av fjärde ordningen av runge-kutta.
Det som är unik med denna kod är att den använder sig av variadic. Detta betyder att man kan variabelt ha utökade argument om så önskas. Jajjemän, precis som Matlab och Python så stöder C detta ända sedan tidernas begynnelse från ANSI C (1989).
Men ett problem! Jag får hardfault när jag kommer till denna kodrad - Varför då? Kan inte variadic köra floatpekare? Min kompilator är GCC för ARM. För MSVC så fungerar det utan problem.
Kod: Markera allt
matrices[i] = va_arg(args, float*);Kod: Markera allt
/******************************************************************************
Online C Compiler.
Code, Compile, Run and Debug C program online.
Write your code in this editor and press "Run" button to compile and execute it.
*******************************************************************************/
#include <string.h> /* For memcpy, memset etc */
#include <stdio.h> /* For printf */
#include <stdlib.h> /* Standard library */
#include <math.h> /* For sqrtf */
#include <float.h> /* Required for FLT_EPSILON, FLT_MAX, FLT_MIN */
#include <stddef.h> /* Requried for size_t, NULL etc... */
#include <time.h> /* For srand, clock */
#include <stdarg.h> /* For ... arguments */
void print(const float A[], const size_t row, const size_t column) {
size_t i, j;
for (i = 0; i < row; i++) {
for (j = 0; j < column; j++) {
printf("%0.7f\t", *(A++));
}
printf("\n");
}
printf("\n");
}
void mul(const float A[], const float B[], float C[], const size_t row_a, const size_t column_a, const size_t column_b){
/* Decleration */
size_t i, j, k;
/* Data matrix */
const float* data_a, * data_b;
for (i = 0; i < row_a; i++) {
/* Then we go through every column of b */
for (j = 0; j < column_b; j++) {
data_a = A;
data_b = &B[j];
C[0] = 0.0f; /* Reset */
/* And we multiply rows from a with columns of b */
for (k = 0; k < column_a; k++) {
*C += data_a[0] * data_b[0];
data_a++;
data_b += column_b;
}
C++; /* ;) */
}
A += column_a;
}
}
/*
* Fourth-order Runge–Kutta method.
* Runge-Kutta method is not the best ODE solver, but it's the best ODE solver for fixed step time iteration.
* h - Step time
* Y[iterations*N] - Output
* y[N] - Initial state vector
* N - Dimension for y-vector
* odefun(const float t, float y[], const float* matrices[], const size_t rows[], const size_t columns[])
* ... = const float matrixA[], const size_t rowA, const size_t columnA, const float matrixB[], const size_t rowB, const size_t columnB, const float matrixC[], const size_t rowC, const size_t columnC, etc...
*/
void rk4args(const size_t iterations, const float h, float Y[], float y[], const size_t N, void (*odefun)(float, float*, float**, const size_t*, const size_t*), const size_t number_of_pointers, ...) {
/* Variables */
size_t i, j;
/* Constants */
const size_t length = N * sizeof(float);
/* Create vectors */
float* k1 = (float*)malloc(length);
float* k2 = (float*)malloc(length);
float* k3 = (float*)malloc(length);
float* k4 = (float*)malloc(length);
/* Temporary vectors */
float* yt = (float*)malloc(length);
/* Variables */
float t = 0.0f;
/* Initial output */
memcpy(Y, y, length);
/* Variable arguments */
va_list args;
va_start(args, number_of_pointers);
/* Pointers */
float** matrices = (float**)malloc(number_of_pointers * sizeof(float*));
size_t* rows = (size_t*)malloc(number_of_pointers * sizeof(size_t));
size_t* columns = (size_t*)malloc(number_of_pointers * sizeof(size_t));
/* Get the arguments */
for (i = 0; i < number_of_pointers; i++) {
matrices[i] = va_arg(args, float*);
rows[i] = va_arg(args, const size_t);
columns[i] = va_arg(args, const size_t);
}
/* Perform Runge-Kutta */
for (i = 1; i < iterations; i++) {
/* Receive old output */
for (j = 0; j < N; j++) {
y[j] = Y[(i - 1) * N + j];
}
/* First statement */
memcpy(yt, y, length);
odefun(t, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k1[j] = yt[j] * h;
}
/* Second statement */
memcpy(yt, y, length);
for (j = 0; j < N; j++) {
yt[j] += k1[j] / 2.0f;
}
odefun(t + h / 2.0f, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k2[j] = yt[j] * h;
}
/* Third statement */
memcpy(yt, y, length);
for (j = 0; j < N; j++) {
yt[j] += k2[j] / 2.0f;
}
odefun(t + h / 2.0f, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k3[j] = yt[j] * h;
}
/* Fourth statement */
memcpy(yt, y, length);
for (j = 0; j < N; j++) {
yt[j] += k3[j];
}
odefun(t + h, yt, matrices, rows, columns);
for (j = 0; j < N; j++) {
k4[j] = yt[j] * h;
}
/* Save output */
for (j = 0; j < N; j++) {
Y[i * N + j] = y[j] + (k1[j] + 2.0f * k2[j] + 2.0f * k3[j] + k4[j]) / 6.0f;
}
/* Update t */
t += h;
}
/* Free */
free(k1);
free(k2);
free(k3);
free(k4);
free(yt);
free(rows);
free(columns);
free(matrices);
/* Close */
va_end(args);
}
/* Constants */
#define N 2
#define h 0.1f
#define row_a 2
#define column_a 2
#define row_b 2
#define column_b 1
#define row_u 1
#define column_u 1
#define ITERATIONS 200
/* Create ODE equation */
void odefun(const float t, float y[], float* matrices[], const size_t rows[], const size_t columns[]) {
/* Get the matrices */
float* A = matrices[0];
float* B = matrices[1];
float* u = matrices[2];
/* Get the sizes */
size_t row_A = rows[0];
size_t column_A = columns[0];
size_t row_B = rows[1];
size_t column_B = columns[1];
size_t row_U = rows[2];
size_t column_U = columns[2];
/* Solve y = A*y + B*u */
float Ay[2];
float Bu[2];
mul(A, y, Ay, row_A, column_A, 1);
mul(B, u, Bu, row_B, column_B, 1);
size_t i;
for (i = 0; i < row_A; i++) {
y[i] = Ay[i] + Bu[i];
}
}
int main() {
clock_t start, end;
float cpu_time_used;
start = clock();
/* Declare intial state */
float y0[N] = { 0 };
/* Declare output */
float Y[ITERATIONS * N];
/* Declare number of pointers */
const size_t number_of_pointers = 3;
const float k = 5.0f; /* Spring constant [N/m] */
const float b = 1.4f; /* Damper constant [Ns/m] */
const float m = 2.0f; /* Mass [kg] */
const float F = 8.0f; /* Force [N] */
const float A[row_a * column_a] = { 0, 1, -k / m, -b / m };
const float B[row_b * column_b] = { 0, 1 / m };
const float u[row_u * column_u] = { F };
/* Perform Runge-Kutta 4:th order with extended arguments ... of const float[], const size_t, const size_t etc... */
rk4args(ITERATIONS, h, Y, y0, N, odefun, number_of_pointers, A, (const size_t)row_a, (const size_t)column_a, B, (const size_t)row_b, (const size_t)column_b, u, (const size_t)row_u, (const size_t)column_u);
end = clock();
cpu_time_used = ((float)(end - start)) / CLOCKS_PER_SEC;
printf("\nTotal speed was %f\n", cpu_time_used);
/* Plot */
print(Y, ITERATIONS, N);
return EXIT_SUCCESS;
}
