0

私は組み込みシステムに非常に慣れていないため、これを記述する方法がわかりませんが、卒業プロジェクトのために状態オブザーバーをコーディングしようとしており、2 つの異なる軸で 3 つの状態を推定する必要があります。

私が解こうとしている基本的な方程式は次のとおりです。

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

問題は、2 番目の軸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 に答える