умножение матрицы умножения arm_math - PullRequest
Новая
       59

умножение матрицы умножения arm_math

0 голосов
/ 31 октября 2019

Не знаю, как написать это, поскольку я очень плохо знаком со встраиваемыми системами, но я пытался написать код наблюдателя состояния для моего дипломного проекта, и ему нужно оценить 3 состояния на 2 разных осях.

Основные уравнения, которые я пытаюсь решить: 

x_hat[k+1] = (A-L*C)*x_hat[k]+ B*u + L*y

Проблема в том, что я получаю HardFault_Handler() при выполнении части (A-L*C)*x_hat[k] для второй оси.

Код работает на одной из осей, но он получает эту ошибку на другой. Они также не работают отдельно.

(Показывается только код для проблемной оси) Код прерывания: (прерывания 800 Гц, dt = 1/800) 

      placeholder[0]=(int16_t)L3GD20_GetAngularRateY(sensitivity_250,0);
      placeholder[1]=(int16_t)L3GD20_GetAngularRateZ(sensitivity_250,0);
      ydata[1]=placeholder[0]*3.14158/180;
      zdata[1]=placeholder[1]*3.14158/180;
      ydata[0]+=ydata[1]*dt;
      zdata[0]+=zdata[1]*dt;
      status = arm_mat_mult_f32(&Czm, &zdatam, &ZCm);         //C*y(estados)


      status = arm_mat_mult_f32(&ALCzm, &zhatantm, &ALCzantm);  //(A-LC)*yhatant
      status = arm_mat_scale_f32(&Bzm, uz, &Bzuzm);             //By*uy
      status = arm_mat_mult_f32(&Lzm, &ZCm, &Lzcm);             //L*Y(saida);
      status = arm_mat_add_f32(&ALCzantm, &Bzuzm, &ALCBzm);     //(A-LC)*yhatant + By*uy
      status = arm_mat_add_f32(&ALCBzm, &Lzcm, &deltazhatm);    //(A-LC)*yhatant + By*uy + L*Y

      //Integrating to obtain states

      zhat[0] += deltazhat[0]*dt;
      zhat[1] += deltazhat[1]*dt;
      zhat[2] += deltazhat[2]*dt;


      zhatant[0] = zhat[0];
      zhatant[1] = zhat[1];
      zhatant[2] = zhat[2];


      //Generate outputs
      status = arm_mat_mult_f32(&zhatm, &Kzm, &outzm);
      uz = -outz[0];

Инициализация матриц: 

      arm_mat_init_f32(&zdatam, 3, 1, (float32_t *) zdata);
      arm_mat_init_f32(&Azm, 3, 3, (float32_t *) Az);
      arm_mat_init_f32(&Bzm, 3, 1, (float32_t *) Bz);
      arm_mat_init_f32(&Czm, 2, 3, (float32_t *) Cz);
      arm_mat_init_f32(&Kzm, 1, 3, (float32_t *) Kz);
      arm_mat_init_f32(&Lzm, 3, 2, (float32_t *) Lz);
      arm_mat_init_f32(&outzm, 1, 1, (float32_t *) outz);
      arm_mat_init_f32(&zhatm, 3, 1, (float32_t *) zhat);
      arm_mat_init_f32(&zhatantm, 3, 1, (float32_t *) zhatant);
      arm_mat_init_f32(&LCzm, 3, 3, (float32_t *) LCz);
      arm_mat_init_f32(&ALCzm, 3, 3, (float32_t *) ALCz);
      arm_mat_init_f32(&ALCzantm, 3, 1, (float32_t *) ALCzant);
      arm_mat_init_f32(&Bzuzm, 3, 1, (float32_t *) Bzuz);
      arm_mat_init_f32(&ZCm, 2, 1, (float32_t *) ZC);
      arm_mat_init_f32(&Lzcm, 3, 1, (float32_t *) Lzc);
      arm_mat_init_f32(&ALCBzm, 3, 1, (float32_t *) ALCBz);
      arm_mat_init_f32(&deltazhatm, 3, 1, (float32_t *) deltazhat);

float32_t ydata[3*1] = {0,0,0};
float32_t zdata[3*1] = {0,0,0};
float32_t placeholder[2*1] = {0,0};
float32_t LCy[3*3];
float32_t LCz[3*3];
float32_t ALCyant[3*1];
float32_t ALCzant[3*1];
float32_t Byuy[3*1];
float32_t Bzuz[3*1];
float32_t YC[2*1];
float32_t ZC[2*1];
float32_t Lyc[3*1];
float32_t Lzc[3*1];
float32_t yhat[3*1];
float32_t zhat[3*1];
float32_t yhatant[3*1];
float32_t zhatant[3*1];
float32_t ALCBy[3*1];
float32_t ALCBz[3*1];
float32_t deltayhat[3*1];
float32_t deltazhat[3*1];
float32_t outy[1*1];
float32_t outz[1*1];
float32_t uy;
float32_t uz;

//Y
const float32_t Ay[3*3] = {0, 1, 0,29.529, 0, 0.17903, -29.529, 0, -29.529};
const float32_t By[3*1] = {0, -3.0396, 4.8226};
const float32_t Cy[3*3] = {1, 0, 0, 0, 1, 0, 0, 0, 0};
const float32_t ALCy[3*3]= {-37.0458, -114.1644, 0, -15.2513, -1535.2383, 0.17903, -3030.1313, -305028.3011, -3030.1313};


//Z
const float32_t Az[3*3] = {0, 1, 0,29.527, 0, 0.17902, -29.527, 0, -29.527};
const float32_t Bz[3*1] = {0, -3.0394, 4.8226};
const float32_t Cz[3*3] = {1, 0, 0, 0, 1, 0};
const float32_t ALCz[3*3]= {-37.4149, -1249.709, 0, -5.5092, -1534.8692, 0.17902, -1140.5251, -330698.2504, -1140.5251};


//system gains
const float32_t Ky[3*1] = {-63.5762, -23.9051, -4.1428};
const float32_t Ly[3*2] = {37.0458, 115.1644, 44.7803, 1535.2383, 3000.6023, 305028.3011};
const float32_t Kz[3*1] = {-63.5806, -23.9066, -4.1424};
const float32_t Lz[3*2] = {37.4149, 1250.709, 35.0362, 1534.8692, 1110.9981, 330698.2504};

1 Ответ

0 голосов
/ 31 октября 2019

В этой строке: 

    status = arm_mat_mult_f32(&zhatm, &Kzm, &outzm);

zhatm - матрица 3x1, а Kzm - матрица 1x3. При умножении вместе получается матрица 3х3, но при условии outzm это матрица 1х1.

Возможно, вам нужно поменять параметры следующим образом: 

    status = arm_mat_mult_f32(&Kzm, &zhatm, &outzm);

Это приведет кматрица 1x1 в outzm.

Если в коде ранее произошла серьезная ошибка, это может быть связано с задержкой эффекта подобных ошибок.

...