0

不知道怎么写，因为我对嵌入式系统很陌生，但我一直在尝试为我的毕业项目编写一个状态观察器，它需要估计 2 个不同轴上的 3 个状态。

我试图解决的基本方程是：

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

问题是我在为第二个轴HardFault_Handler()做零件时遇到了问题。(A-L*C)*x_hat[k]

该代码在一个轴上工作，但在另一个轴上出现此错误。它们也不单独工作。

（仅显示有问题的轴的代码）中断代码：（800hz 中断，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};
4

1 回答 1

0

在这一行：

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

zhatm是一个 3x1 矩阵，Kzm是一个 1x3 矩阵。相乘时，结果是 3x3 矩阵，但提供outzm的是 1x1 矩阵。

也许您需要按如下方式交换参数：

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

这将在 中产生一个 1x1 矩阵outzm。

如果硬故障发生在代码的早期，那可能是由于类似错误的延迟影响。

于 2019-10-31T15:57:12.520 回答