This forum is now READ-ONLY! Please visit http://forum.wiring.co/ for the new forum.

Wiring ForumProgramming Questions & HelpSyntax › Performing Matrix Operations

 Pages: 1
 Performing Matrix Operations (Read 7341 times)
 01/11/07 at 02:52:45
 CSMotionControl   Offline YaBB Newbies Hey, did you get any mail for Willie Nelson? Posts: 16
 Anybody have any neat examples to direct me to?  I'm not even really sure what I want to do, other than use them.      IP Logged
 Reply #1 - 01/15/07 at 00:17:36
 CSMotionControl   Offline YaBB Newbies Hey, did you get any mail for Willie Nelson? Posts: 16
 Specifically, something like the following:Pdot[2 * 2] = {Q_angle - P[0][1] - P[1][0],- P[1][1], - P[1][1], Q_gyro};Essentially, I'm looking to implement a Kalman filter on a gyro/accelerometer board that I have, but haven't found any clear matrix operations yet.Thanks!   IP Logged
 Reply #2 - 01/15/07 at 07:56:52
 barragan   Offline YaBB Administrator Posts: 939
 this seems to work, I am not sure about your question:int P[6][6]; //I guess you need to put some values here firstint Q_angle = 0;int Q_gyro = 0;int Pdot[2 * 2] = {Q_angle - P[0][1] - P[1][0],- P[1][1], - P[1][1], Q_gyro}; void setup() {// ...} void loop() {// ...// if you want to change values in here for Pdot then this is the way:// Pdot[0] = Q_angle - P[0][1] - P[1][0];// Pdot[1] = - P[1][1];// Pdot[2] = - P[1][1];// Pdot[3] =  Q_gyro;}   IP Logged
 Reply #3 - 01/16/07 at 00:27:07
 CSMotionControl   Offline YaBB Newbies Hey, did you get any mail for Willie Nelson? Posts: 16
 This is the specific code I was looking to implement:#include static const float      dt      = ( 1024.0 * 256.0 ) / 8000000.0;static float            P[2][2] = {      { 1, 0 },      { 0, 1 },};float                  angle;float                  q_bias;float                  rate;static const float      R_angle      = 0.3;static const float      Q_angle      = 0.001;static const float      Q_gyro      = 0.003;voidstate_update(const float            q_m){      const float            q = q_m - q_bias;      const float            Pdot[2 * 2] = {            Q_angle - P[0][1] - P[1][0],                          - P[1][1],                                - P[1][1],                              Q_gyro };            rate = q;      angle += q * dt;      P[0][0] += Pdot[0] * dt;      P[0][1] += Pdot[1] * dt;      P[1][0] += Pdot[2] * dt;      P[1][1] += Pdot[3] * dt;}voidkalman_update(const float ax_m,const float az_m){            const float            angle_m = atan2( -az_m, ax_m );      const float            angle_err = angle_m - angle;      const float            C_0 = 1;            const float            PCt_0 = C_0 * P[0][0];      const float            PCt_1 = C_0 * P[1][0];                  const float            E = R_angle + C_0 * PCt_0;      const float            K_0 = PCt_0 / E;      const float            K_1 = PCt_1 / E;                  const float            t_0 = PCt_0;      const float            t_1 = C_0 * P[0][1];      P[0][0] -= K_0 * t_0;      P[0][1] -= K_0 * t_1;      P[1][0] -= K_1 * t_0;      P[1][1] -= K_1 * t_1;            angle      += K_0 * angle_err;      q_bias      += K_1 * angle_err;}I was just looking for the appropriate syntax to use for all the matrix math for Wiring.  I wasn't sure how that differed from other variants of C.   IP Logged
 Reply #4 - 01/17/07 at 07:14:46
 barragan   Offline YaBB Administrator Posts: 939
 syntax is not different, internally wiring is implemented in C/C++   IP Logged
 Reply #5 - 01/17/07 at 07:15:06
 Alan_Kilian   Offline Full Member Wiring? It looks like Java to me. Posts: 118
 I think your code should work fine (matrix-wise anyway). Did you try it and have problems?   IP Logged
 Reply #6 - 01/19/07 at 21:42:10
 CSMotionControl   Offline YaBB Newbies Hey, did you get any mail for Willie Nelson? Posts: 16
 Alan,I haven't tried to implement the code yet, because I don't really have a good way to control the integration period (I'd like to use internal interrupts, but am somewhat stalled on that front).  I am working on running the code right now, fudging the integration period until I'm "close", and then trying to run and tune the filter. Any ideas to simplify or make an easily controlled sample period?   IP Logged
 Pages: 1