Home \ Help \ Search \ Login RSS

Welcome, Guest. Please Login

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

Wiring ForumProgramming Questions & HelpSyntax › Performing Matrix Operations

Page Index Toggle Pages: 1
Performing Matrix Operations (Read 6704 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.   Cheesy
 
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 first
int 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 <math.h>


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;

void
state_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;
}


void
kalman_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
 
Page Index Toggle Pages: 1