diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp
index 30c28f76a1..87409d8474 100644
--- a/Tools/CPUInfo/CPUInfo.cpp
+++ b/Tools/CPUInfo/CPUInfo.cpp
@@ -10,6 +10,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_Math/AP_Math.h>
+#include "EKF_Maths.h"
 
 void setup();
 void loop();
@@ -24,7 +25,12 @@ static uint32_t sysclk = STM32_SYSCLK;
 static uint32_t sysclk = 0;
 #endif
 
+static EKF_Maths ekf;
+
+
+
 void setup() {
+    ekf.init();
 }
 
 static void show_sizes(void)
@@ -126,6 +132,7 @@ static void show_timings(void)
     TIMEIT("sq()",v_out = sq(v_f), 20);
     TIMEIT("powf(v,2)",v_out = powf(v_f, 2), 20);
     TIMEIT("powf(v,3.1)",v_out = powf(v_f, 3.1), 20);
+    TIMEIT("EKF",v_out = ekf.test(), 5);
 
     TIMEIT("iadd8", v_out_8 += v_8, 100);
     TIMEIT("isub8", v_out_8 -= v_8, 100);
diff --git a/Tools/CPUInfo/EKF_Maths.cpp b/Tools/CPUInfo/EKF_Maths.cpp
new file mode 100644
index 0000000000..a48c6c57d5
--- /dev/null
+++ b/Tools/CPUInfo/EKF_Maths.cpp
@@ -0,0 +1,121 @@
+#include "EKF_Maths.h"
+
+/*
+  code for measuring speed of EKF mag fusion code
+ */
+
+static float RandFloat(void)
+{
+    return float(get_random16()+1.0) / 0xFFFF;
+}
+
+void EKF_Maths::init(void)
+{
+    for (uint8_t i=0; i<24; i++) {
+        P[i][i] = RandFloat();
+    }
+    for (uint8_t i=0; i<28; i++) {
+        statesArray[i] = RandFloat();
+    }
+    float *m = (float *)&mag_state;
+    for (uint8_t i=0; i<sizeof(mag_state)/sizeof(ftype); i++) {
+        m[i] = RandFloat();
+    }
+}
+
+float EKF_Maths::test(void)
+{
+    ftype &q0 = mag_state.q0;
+    ftype &q1 = mag_state.q1;
+    ftype &q2 = mag_state.q2;
+    ftype &q3 = mag_state.q3;
+    ftype &magN = mag_state.magN;
+    ftype &magE = mag_state.magE;
+    ftype &magD = mag_state.magD;
+    ftype &magXbias = mag_state.magXbias;
+    ftype &magYbias = mag_state.magYbias;
+    ftype &magZbias = mag_state.magZbias;
+    Matrix3f &DCM = mag_state.DCM;
+    Vector3f &MagPred = mag_state.MagPred;
+    ftype &R_MAG = mag_state.R_MAG;
+    ftype *SH_MAG = &mag_state.SH_MAG[0];
+    Vector6 SK_MX;
+
+    q0       = stateStruct.quat[0];
+    q1       = stateStruct.quat[1];
+    q2       = stateStruct.quat[2];
+    q3       = stateStruct.quat[3];
+    magN     = stateStruct.earth_magfield[0];
+    magE     = stateStruct.earth_magfield[1];
+    magD     = stateStruct.earth_magfield[2];
+    magXbias = stateStruct.body_magfield[0];
+    magYbias = stateStruct.body_magfield[1];
+    magZbias = stateStruct.body_magfield[2];
+
+    DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+    DCM[0][1] = 2.0f*(q1*q2 + q0*q3);
+    DCM[0][2] = 2.0f*(q1*q3-q0*q2);
+    DCM[1][0] = 2.0f*(q1*q2 - q0*q3);
+    DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+    DCM[1][2] = 2.0f*(q2*q3 + q0*q1);
+    DCM[2][0] = 2.0f*(q1*q3 + q0*q2);
+    DCM[2][1] = 2.0f*(q2*q3 - q0*q1);
+    DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+    MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE  + DCM[0][2]*magD + magXbias;
+    MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE  + DCM[1][2]*magD + magYbias;
+    MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE  + DCM[2][2]*magD + magZbias;
+
+    // calculate common expressions used to calculate observation jacobians an innovation variance for each component
+    SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
+    SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+    SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+    SH_MAG[3] = 2.0f*q0*q1 + 2.0f*q2*q3;
+    SH_MAG[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
+    SH_MAG[5] = 2.0f*q0*q2 + 2.0f*q1*q3;
+    SH_MAG[6] = magE*(2.0f*q0*q1 - 2.0f*q2*q3);
+    SH_MAG[7] = 2.0f*q1*q3 - 2.0f*q0*q2;
+    SH_MAG[8] = 2.0f*q0*q3;
+
+    // Calculate the innovation variance for each axis
+    // X axis
+    varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))));
+
+    // Y axis
+    varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2.0f*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2.0f*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2.0f*q1*q2)) - P[16][20]*(SH_MAG[8] - 2.0f*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2.0f*q1*q2)));
+
+    // Z axis
+    varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3)));
+
+    // calculate Kalman gain
+    SK_MX[0] = 1.0f / varInnovMag[0];
+    SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
+    SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
+    SK_MX[3] = SH_MAG[7];
+    Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
+    Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
+    Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
+    Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
+    Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
+    Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
+    Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
+    Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
+    Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
+    Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
+    Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
+    Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
+    Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
+    Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
+    Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
+    Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
+
+    float ret = 0;
+    for (uint8_t i=0; i<16; i++) {
+        ret += Kfusion[i];
+    }
+    for (uint8_t i=0; i<24; i++) {
+        for (uint8_t j=0; j<24; j++) {
+            ret += P[i][j];
+        }
+    }
+    return ret;
+}
diff --git a/Tools/CPUInfo/EKF_Maths.h b/Tools/CPUInfo/EKF_Maths.h
new file mode 100644
index 0000000000..017fae43a3
--- /dev/null
+++ b/Tools/CPUInfo/EKF_Maths.h
@@ -0,0 +1,82 @@
+#pragma once
+
+/*
+  data structure for measuring speed of EKF mag fusion code
+ */
+#include <AP_Math/AP_Math.h>
+#include <stdint.h>
+
+typedef float ftype;
+
+class EKF_Maths {
+public:
+    EKF_Maths() {}
+
+    typedef ftype Vector2[2];
+    typedef ftype Vector3[3];
+    typedef ftype Vector4[4];
+    typedef ftype Vector5[5];
+    typedef ftype Vector6[6];
+    typedef ftype Vector7[7];
+    typedef ftype Vector8[8];
+    typedef ftype Vector9[9];
+    typedef ftype Vector10[10];
+    typedef ftype Vector11[11];
+    typedef ftype Vector13[13];
+    typedef ftype Vector14[14];
+    typedef ftype Vector15[15];
+    typedef ftype Vector22[22];
+    typedef ftype Vector23[23];
+    typedef ftype Vector24[24];
+    typedef ftype Vector25[25];
+    typedef ftype Vector28[28];
+    typedef ftype Matrix3[3][3];
+    typedef ftype Matrix24[24][24];
+    typedef ftype Matrix34_50[34][50];
+    typedef uint32_t Vector_u32_50[50];
+
+    struct state_elements {
+        Vector3f    angErr;         // 0..2
+        Vector3f    velocity;       // 3..5
+        Vector3f    position;       // 6..8
+        Vector3f    gyro_bias;      // 9..11
+        Vector3f    gyro_scale;     // 12..14
+        float       accel_zbias;    // 15
+        Vector3f    earth_magfield; // 16..18
+        Vector3f    body_magfield;  // 19..21
+        Vector2f    wind_vel;       // 22..23
+        Quaternion  quat;           // 24..27
+    };
+
+    union {
+        Vector28 statesArray;
+        struct state_elements stateStruct;
+    };
+
+    struct {
+        ftype q0;
+        ftype q1;
+        ftype q2;
+        ftype q3;
+        ftype magN;
+        ftype magE;
+        ftype magD;
+        ftype magXbias;
+        ftype magYbias;
+        ftype magZbias;
+        Matrix3f DCM;
+        Vector3f MagPred;
+        ftype R_MAG;
+        Vector9 SH_MAG;
+    } mag_state;
+
+    Vector3f innovMag;
+    Vector3f varInnovMag;
+
+    Matrix24 P;
+    Vector28 Kfusion;
+
+    void init(void);
+    float test(void);
+};
+