[英]arm_math matrix multiplication hardfault
Not sure how to write this, since I'm very new to embedded systems, but I've been trying to code a state-observer for my graduation project and it needs to estimate 3 states on 2 different axis.不知道怎么写,因为我对嵌入式系统很陌生,但我一直在尝试为我的毕业项目编写一个状态观察器,它需要估计 2 个不同轴上的 3 个状态。
The basic equations I'm trying to solve are:我试图解决的基本方程是:
x_hat[k+1] = (A-L*C)*x_hat[k]+ B*u + L*y
The problem is I'm getting HardFault_Handler()
when doing the (AL*C)*x_hat[k]
part for the second axis.问题是我在为第二个轴执行
(AL*C)*x_hat[k]
部分时得到HardFault_Handler()
。
The code works on one of the axis, but it gets this error on the other.该代码在一个轴上工作,但在另一个轴上出现此错误。 They don't work separately either.
它们也不单独工作。
(Only showing code for the problematic axis) Interrupt code: (800hz interrupts, dt = 1/800) (仅显示问题轴的代码)中断代码:(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];
Initializing the matrices:初始化矩阵:
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};
In this line:在这一行:
status = arm_mat_mult_f32(&zhatm, &Kzm, &outzm);
zhatm
is a 3x1 matrix and Kzm
is a 1x3 matrix. zhatm
是一个 3x1 矩阵,而Kzm
是一个 1x3 矩阵。 When multiplied together, the result is a 3x3 matrix, but the provided outzm
is a 1x1 matrix.相乘时,结果是一个 3x3 矩阵,但提供的
outzm
是一个 1x1 矩阵。
Perhaps you need to swap the parameters as follows:也许您需要按如下方式交换参数:
status = arm_mat_mult_f32(&Kzm, &zhatm, &outzm);
That will produce a 1x1 matrix in outzm
.这将在
outzm
中产生一个 1x1 矩阵。
If the hard fault occurs earlier in the code, that might be due to the delayed effect of similar errors.如果硬故障发生在代码的早期,那可能是由于类似错误的延迟影响。
声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.