#include <math.h>
#include <stdio.h>
#include <cstdlib>
#include "../../../../../matrix/matrix/math.hpp"
#include "util.h"

typedef matrix::Vector<float, 24> Vector24f;
typedef matrix::SquareMatrix<float, 24> SquareMatrix24f;

template<int ... Idxs>
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;

int main()
{
    // Compare calculation of observation Jacobians and Kalman gains for sympy and matlab generated equations

	SparseVector24f<0,1,2,3,4,5,6> Hfusion; // Optical flow observation Jacobians
    Vector24f Kfusion; // Optical flow observation Kalman gains

    Vector24f Hfusion_sympy;
    Vector24f Kfusion_sympy;

    Vector24f Hfusion_matlab;
    Vector24f Kfusion_matlab;

	const float R_LOS = sq(0.15f);

    float flow_innov_var;

    // quaternion inputs must be normalised
    float q0 = 2.0f * ((float)rand() - 0.5f);
    float q1 = 2.0f * ((float)rand() - 0.5f);
    float q2 = 2.0f * ((float)rand() - 0.5f);
    float q3 = 2.0f * ((float)rand() - 0.5f);
    const float length = sqrtf(sq(q0) + sq(q1) + sq(q2) + sq(q3));
    q0 /= length;
    q1 /= length;
    q2 /= length;
    q3 /= length;

	// get latest velocity in earth frame
	const float vn = 10.0f * 2.0f * ((float)rand() - 0.5f);
	const float ve = 10.0f * 2.0f * ((float)rand() - 0.5f);
	const float vd = 2.0f * ((float)rand() - 0.5f);

    const float range = 5.0f;

    matrix::Dcmf Tbs;
	Tbs.identity();

    // create a symmetrical positive definite matrix with off diagonals between -1 and 1 and diagonals between 0 and 1
    SquareMatrix24f P;
    for (int col=0; col<=23; col++) {
        for (int row=0; row<=col; row++) {
            if (row == col) {
                P(row,col) = (float)rand();
            } else {
                P(col,row) = P(row,col) = 2.0f * ((float)rand() - 0.5f);
            }
        }
    }

    // evaluate sub expressions used by sympy code
    const float HK0 = -Tbs(1,0)*q2 + Tbs(1,1)*q1 + Tbs(1,2)*q0;
    const float HK1 = Tbs(1,0)*q3 + Tbs(1,1)*q0 - Tbs(1,2)*q1;
    const float HK2 = Tbs(1,0)*q0 - Tbs(1,1)*q3 + Tbs(1,2)*q2;
    const float HK3 = HK0*vd + HK1*ve + HK2*vn;
    const float HK4 = 1.0F/range;
    const float HK5 = 2*HK4;
    const float HK6 = Tbs(1,0)*q1 + Tbs(1,1)*q2 + Tbs(1,2)*q3;
    const float HK7 = -HK0*ve + HK1*vd + HK6*vn;
    const float HK8 = HK0*vn - HK2*vd + HK6*ve;
    const float HK9 = -HK1*vn + HK2*ve + HK6*vd;
    const float HK10 = q0*q2;
    const float HK11 = q1*q3;
    const float HK12 = HK10 + HK11;
    const float HK13 = 2*Tbs(1,2);
    const float HK14 = q0*q3;
    const float HK15 = q1*q2;
    const float HK16 = HK14 - HK15;
    const float HK17 = 2*Tbs(1,1);
    const float HK18 = ecl::powf(q1, 2);
    const float HK19 = ecl::powf(q2, 2);
    const float HK20 = -HK19;
    const float HK21 = ecl::powf(q0, 2);
    const float HK22 = ecl::powf(q3, 2);
    const float HK23 = HK21 - HK22;
    const float HK24 = HK18 + HK20 + HK23;
    const float HK25 = HK12*HK13 - HK16*HK17 + HK24*Tbs(1,0);
    const float HK26 = HK14 + HK15;
    const float HK27 = 2*Tbs(1,0);
    const float HK28 = q0*q1;
    const float HK29 = q2*q3;
    const float HK30 = HK28 - HK29;
    const float HK31 = -HK18;
    const float HK32 = HK19 + HK23 + HK31;
    const float HK33 = -HK13*HK30 + HK26*HK27 + HK32*Tbs(1,1);
    const float HK34 = HK28 + HK29;
    const float HK35 = HK10 - HK11;
    const float HK36 = HK20 + HK21 + HK22 + HK31;
    const float HK37 = HK17*HK34 - HK27*HK35 + HK36*Tbs(1,2);
    const float HK38 = 2*HK3;
    const float HK39 = 2*HK7;
    const float HK40 = 2*HK8;
    const float HK41 = 2*HK9;
    const float HK42 = HK25*P(0,4) + HK33*P(0,5) + HK37*P(0,6) + HK38*P(0,0) + HK39*P(0,1) + HK40*P(0,2) + HK41*P(0,3);
    const float HK43 = ecl::powf(range, -2);
    const float HK44 = HK25*P(4,6) + HK33*P(5,6) + HK37*P(6,6) + HK38*P(0,6) + HK39*P(1,6) + HK40*P(2,6) + HK41*P(3,6);
    const float HK45 = HK25*P(4,5) + HK33*P(5,5) + HK37*P(5,6) + HK38*P(0,5) + HK39*P(1,5) + HK40*P(2,5) + HK41*P(3,5);
    const float HK46 = HK25*P(4,4) + HK33*P(4,5) + HK37*P(4,6) + HK38*P(0,4) + HK39*P(1,4) + HK40*P(2,4) + HK41*P(3,4);
    const float HK47 = HK25*P(2,4) + HK33*P(2,5) + HK37*P(2,6) + HK38*P(0,2) + HK39*P(1,2) + HK40*P(2,2) + HK41*P(2,3);
    const float HK48 = HK25*P(3,4) + HK33*P(3,5) + HK37*P(3,6) + HK38*P(0,3) + HK39*P(1,3) + HK40*P(2,3) + HK41*P(3,3);
    const float HK49 = HK25*P(1,4) + HK33*P(1,5) + HK37*P(1,6) + HK38*P(0,1) + HK39*P(1,1) + HK40*P(1,2) + HK41*P(1,3);
    const float HK50 = HK4/(HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);

    const float HK51 = Tbs(0,1)*q1;
    const float HK52 = Tbs(0,2)*q0;
    const float HK53 = Tbs(0,0)*q2;
    const float HK54 = HK51 + HK52 - HK53;
    const float HK55 = Tbs(0,0)*q3;
    const float HK56 = Tbs(0,1)*q0;
    const float HK57 = Tbs(0,2)*q1;
    const float HK58 = HK55 + HK56 - HK57;
    const float HK59 = Tbs(0,0)*q0;
    const float HK60 = Tbs(0,2)*q2;
    const float HK61 = Tbs(0,1)*q3;
    const float HK62 = HK59 + HK60 - HK61;
    const float HK63 = HK54*vd + HK58*ve + HK62*vn;
    const float HK64 = Tbs(0,0)*q1 + Tbs(0,1)*q2 + Tbs(0,2)*q3;
    const float HK65 = HK58*vd + HK64*vn;
    const float HK66 = -HK54*ve + HK65;
    const float HK67 = HK54*vn + HK64*ve;
    const float HK68 = -HK62*vd + HK67;
    const float HK69 = HK62*ve + HK64*vd;
    const float HK70 = -HK58*vn + HK69;
    const float HK71 = 2*Tbs(0,1);
    const float HK72 = 2*Tbs(0,2);
    const float HK73 = HK12*HK72 + HK24*Tbs(0,0);
    const float HK74 = -HK16*HK71 + HK73;
    const float HK75 = 2*Tbs(0,0);
    const float HK76 = HK26*HK75 + HK32*Tbs(0,1);
    const float HK77 = -HK30*HK72 + HK76;
    const float HK78 = HK34*HK71 + HK36*Tbs(0,2);
    const float HK79 = -HK35*HK75 + HK78;
    const float HK80 = 2*HK63;
    const float HK81 = 2*HK65 + 2*ve*(-HK51 - HK52 + HK53);
    const float HK82 = 2*HK67 + 2*vd*(-HK59 - HK60 + HK61);
    const float HK83 = 2*HK69 + 2*vn*(-HK55 - HK56 + HK57);
    const float HK84 = HK71*(-HK14 + HK15) + HK73;
    const float HK85 = HK72*(-HK28 + HK29) + HK76;
    const float HK86 = HK75*(-HK10 + HK11) + HK78;
    const float HK87 = HK80*P(0,0) + HK81*P(0,1) + HK82*P(0,2) + HK83*P(0,3) + HK84*P(0,4) + HK85*P(0,5) + HK86*P(0,6);
    const float HK88 = HK80*P(0,6) + HK81*P(1,6) + HK82*P(2,6) + HK83*P(3,6) + HK84*P(4,6) + HK85*P(5,6) + HK86*P(6,6);
    const float HK89 = HK80*P(0,5) + HK81*P(1,5) + HK82*P(2,5) + HK83*P(3,5) + HK84*P(4,5) + HK85*P(5,5) + HK86*P(5,6);
    const float HK90 = HK80*P(0,4) + HK81*P(1,4) + HK82*P(2,4) + HK83*P(3,4) + HK84*P(4,4) + HK85*P(4,5) + HK86*P(4,6);
    const float HK91 = HK80*P(0,2) + HK81*P(1,2) + HK82*P(2,2) + HK83*P(2,3) + HK84*P(2,4) + HK85*P(2,5) + HK86*P(2,6);
    const float HK92 = 2*HK43;
    const float HK93 = HK80*P(0,3) + HK81*P(1,3) + HK82*P(2,3) + HK83*P(3,3) + HK84*P(3,4) + HK85*P(3,5) + HK86*P(3,6);
    const float HK94 = HK80*P(0,1) + HK81*P(1,1) + HK82*P(1,2) + HK83*P(1,3) + HK84*P(1,4) + HK85*P(1,5) + HK86*P(1,6);
    const float HK95 = HK4/(HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);


    // Compare X axis equations
    {
        // evaluate sympy genrated equations for observatio Jacobians and Kalman gains
        {
            // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
            flow_innov_var = (HK25*HK43*HK46 + HK33*HK43*HK45 + HK37*HK43*HK44 + HK38*HK42*HK43 + HK39*HK43*HK49 + HK40*HK43*HK47 + HK41*HK43*HK48 + R_LOS);

            const float HK50 = HK4/flow_innov_var;

            // Observation Jacobians - axis 0
            Hfusion.at<0>() = HK3*HK5;
            Hfusion.at<1>() = HK5*HK7;
            Hfusion.at<2>() = HK5*HK8;
            Hfusion.at<3>() = HK5*HK9;
            Hfusion.at<4>() = HK25*HK4;
            Hfusion.at<5>() = HK33*HK4;
            Hfusion.at<6>() = HK37*HK4;

			// Kalman gains - axis 0
			Kfusion(0) = HK42*HK50;
			Kfusion(1) = HK49*HK50;
			Kfusion(2) = HK47*HK50;
			Kfusion(3) = HK48*HK50;
			Kfusion(4) = HK46*HK50;
			Kfusion(5) = HK45*HK50;
			Kfusion(6) = HK44*HK50;

			for (unsigned row = 7; row <= 23; row++) {
				Kfusion(row) = HK50*(HK25*P(4,row) + HK33*P(5,row) + HK37*P(6,row) + HK38*P(0,row) + HK39*P(1,row) + HK40*P(2,row) + HK41*P(3,row));
			}

            // copy to arrays used for comparison
            for (int row=0; row<7; row++) {
                Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
            }
            for (int row=0; row<24; row++) {
                Kfusion_sympy(row) = Kfusion(row);
            }

        }

        // repeat calculation using matlab generated equations

        {
            // calculate X axis observation Jacobian
            float t2 = 1.0f / range;
            float H_LOS[24] = {};
            H_LOS[0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
            H_LOS[1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
            H_LOS[2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
            H_LOS[3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
            H_LOS[4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
            H_LOS[5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
            H_LOS[6] = t2*(q0*q1*2.0f+q2*q3*2.0f);

            // calculate intermediate variables for the X observation innovatoin variance and Kalman gains
            float t3 = q1*vd*2.0f;
            float t4 = q0*ve*2.0f;
            float t11 = q3*vn*2.0f;
            float t5 = t3+t4-t11;
            float t6 = q0*q3*2.0f;
            float t29 = q1*q2*2.0f;
            float t7 = t6-t29;
            float t8 = q0*q1*2.0f;
            float t9 = q2*q3*2.0f;
            float t10 = t8+t9;
            float t12 = P(0,0)*t2*t5;
            float t13 = q0*vd*2.0f;
            float t14 = q2*vn*2.0f;
            float t28 = q1*ve*2.0f;
            float t15 = t13+t14-t28;
            float t16 = q3*vd*2.0f;
            float t17 = q2*ve*2.0f;
            float t18 = q1*vn*2.0f;
            float t19 = t16+t17+t18;
            float t20 = q3*ve*2.0f;
            float t21 = q0*vn*2.0f;
            float t30 = q2*vd*2.0f;
            float t22 = t20+t21-t30;
            float t23 = q0*q0;
            float t24 = q1*q1;
            float t25 = q2*q2;
            float t26 = q3*q3;
            float t27 = t23-t24+t25-t26;
            float t31 = P(1,1)*t2*t15;
            float t32 = P(6,0)*t2*t10;
            float t33 = P(1,0)*t2*t15;
            float t34 = P(2,0)*t2*t19;
            float t35 = P(5,0)*t2*t27;
            float t79 = P(4,0)*t2*t7;
            float t80 = P(3,0)*t2*t22;
            float t36 = t12+t32+t33+t34+t35-t79-t80;
            float t37 = t2*t5*t36;
            float t38 = P(6,1)*t2*t10;
            float t39 = P(0,1)*t2*t5;
            float t40 = P(2,1)*t2*t19;
            float t41 = P(5,1)*t2*t27;
            float t81 = P(4,1)*t2*t7;
            float t82 = P(3,1)*t2*t22;
            float t42 = t31+t38+t39+t40+t41-t81-t82;
            float t43 = t2*t15*t42;
            float t44 = P(6,2)*t2*t10;
            float t45 = P(0,2)*t2*t5;
            float t46 = P(1,2)*t2*t15;
            float t47 = P(2,2)*t2*t19;
            float t48 = P(5,2)*t2*t27;
            float t83 = P(4,2)*t2*t7;
            float t84 = P(3,2)*t2*t22;
            float t49 = t44+t45+t46+t47+t48-t83-t84;
            float t50 = t2*t19*t49;
            float t51 = P(6,3)*t2*t10;
            float t52 = P(0,3)*t2*t5;
            float t53 = P(1,3)*t2*t15;
            float t54 = P(2,3)*t2*t19;
            float t55 = P(5,3)*t2*t27;
            float t85 = P(4,3)*t2*t7;
            float t86 = P(3,3)*t2*t22;
            float t56 = t51+t52+t53+t54+t55-t85-t86;
            float t57 = P(6,5)*t2*t10;
            float t58 = P(0,5)*t2*t5;
            float t59 = P(1,5)*t2*t15;
            float t60 = P(2,5)*t2*t19;
            float t61 = P(5,5)*t2*t27;
            float t88 = P(4,5)*t2*t7;
            float t89 = P(3,5)*t2*t22;
            float t62 = t57+t58+t59+t60+t61-t88-t89;
            float t63 = t2*t27*t62;
            float t64 = P(6,4)*t2*t10;
            float t65 = P(0,4)*t2*t5;
            float t66 = P(1,4)*t2*t15;
            float t67 = P(2,4)*t2*t19;
            float t68 = P(5,4)*t2*t27;
            float t90 = P(4,4)*t2*t7;
            float t91 = P(3,4)*t2*t22;
            float t69 = t64+t65+t66+t67+t68-t90-t91;
            float t70 = P(6,6)*t2*t10;
            float t71 = P(0,6)*t2*t5;
            float t72 = P(1,6)*t2*t15;
            float t73 = P(2,6)*t2*t19;
            float t74 = P(5,6)*t2*t27;
            float t93 = P(4,6)*t2*t7;
            float t94 = P(3,6)*t2*t22;
            float t75 = t70+t71+t72+t73+t74-t93-t94;
            float t76 = t2*t10*t75;
            float t87 = t2*t22*t56;
            float t92 = t2*t7*t69;
            float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
            float t78 = 1.0f / t77;
            flow_innov_var = t77;

            // calculate Kalman gains for X-axis observation
            float Kfusion[24];
            Kfusion[0] = t78*(t12-P(0,4)*t2*t7+P(0,1)*t2*t15+P(0,6)*t2*t10+P(0,2)*t2*t19-P(0,3)*t2*t22+P(0,5)*t2*t27);
            Kfusion[1] = t78*(t31+P(1,0)*t2*t5-P(1,4)*t2*t7+P(1,6)*t2*t10+P(1,2)*t2*t19-P(1,3)*t2*t22+P(1,5)*t2*t27);
            Kfusion[2] = t78*(t47+P(2,0)*t2*t5-P(2,4)*t2*t7+P(2,1)*t2*t15+P(2,6)*t2*t10-P(2,3)*t2*t22+P(2,5)*t2*t27);
            Kfusion[3] = t78*(-t86+P(3,0)*t2*t5-P(3,4)*t2*t7+P(3,1)*t2*t15+P(3,6)*t2*t10+P(3,2)*t2*t19+P(3,5)*t2*t27);
            Kfusion[4] = t78*(-t90+P(4,0)*t2*t5+P(4,1)*t2*t15+P(4,6)*t2*t10+P(4,2)*t2*t19-P(4,3)*t2*t22+P(4,5)*t2*t27);
            Kfusion[5] = t78*(t61+P(5,0)*t2*t5-P(5,4)*t2*t7+P(5,1)*t2*t15+P(5,6)*t2*t10+P(5,2)*t2*t19-P(5,3)*t2*t22);
            Kfusion[6] = t78*(t70+P(6,0)*t2*t5-P(6,4)*t2*t7+P(6,1)*t2*t15+P(6,2)*t2*t19-P(6,3)*t2*t22+P(6,5)*t2*t27);
            Kfusion[7] = t78*(P(7,0)*t2*t5-P(7,4)*t2*t7+P(7,1)*t2*t15+P(7,6)*t2*t10+P(7,2)*t2*t19-P(7,3)*t2*t22+P(7,5)*t2*t27);
            Kfusion[8] = t78*(P(8,0)*t2*t5-P(8,4)*t2*t7+P(8,1)*t2*t15+P(8,6)*t2*t10+P(8,2)*t2*t19-P(8,3)*t2*t22+P(8,5)*t2*t27);
            Kfusion[9] = t78*(P(9,0)*t2*t5-P(9,4)*t2*t7+P(9,1)*t2*t15+P(9,6)*t2*t10+P(9,2)*t2*t19-P(9,3)*t2*t22+P(9,5)*t2*t27);
            Kfusion[10] = t78*(P(10,0)*t2*t5-P(10,4)*t2*t7+P(10,1)*t2*t15+P(10,6)*t2*t10+P(10,2)*t2*t19-P(10,3)*t2*t22+P(10,5)*t2*t27);
            Kfusion[11] = t78*(P(11,0)*t2*t5-P(11,4)*t2*t7+P(11,1)*t2*t15+P(11,6)*t2*t10+P(11,2)*t2*t19-P(11,3)*t2*t22+P(11,5)*t2*t27);
            Kfusion[12] = t78*(P(12,0)*t2*t5-P(12,4)*t2*t7+P(12,1)*t2*t15+P(12,6)*t2*t10+P(12,2)*t2*t19-P(12,3)*t2*t22+P(12,5)*t2*t27);
            Kfusion[13] = t78*(P(13,0)*t2*t5-P(13,4)*t2*t7+P(13,1)*t2*t15+P(13,6)*t2*t10+P(13,2)*t2*t19-P(13,3)*t2*t22+P(13,5)*t2*t27);
            Kfusion[14] = t78*(P(14,0)*t2*t5-P(14,4)*t2*t7+P(14,1)*t2*t15+P(14,6)*t2*t10+P(14,2)*t2*t19-P(14,3)*t2*t22+P(14,5)*t2*t27);
            Kfusion[15] = t78*(P(15,0)*t2*t5-P(15,4)*t2*t7+P(15,1)*t2*t15+P(15,6)*t2*t10+P(15,2)*t2*t19-P(15,3)*t2*t22+P(15,5)*t2*t27);
            Kfusion[16] = t78*(P(16,0)*t2*t5-P(16,4)*t2*t7+P(16,1)*t2*t15+P(16,6)*t2*t10+P(16,2)*t2*t19-P(16,3)*t2*t22+P(16,5)*t2*t27);
            Kfusion[17] = t78*(P(17,0)*t2*t5-P(17,4)*t2*t7+P(17,1)*t2*t15+P(17,6)*t2*t10+P(17,2)*t2*t19-P(17,3)*t2*t22+P(17,5)*t2*t27);
            Kfusion[18] = t78*(P(18,0)*t2*t5-P(18,4)*t2*t7+P(18,1)*t2*t15+P(18,6)*t2*t10+P(18,2)*t2*t19-P(18,3)*t2*t22+P(18,5)*t2*t27);
            Kfusion[19] = t78*(P(19,0)*t2*t5-P(19,4)*t2*t7+P(19,1)*t2*t15+P(19,6)*t2*t10+P(19,2)*t2*t19-P(19,3)*t2*t22+P(19,5)*t2*t27);
            Kfusion[20] = t78*(P(20,0)*t2*t5-P(20,4)*t2*t7+P(20,1)*t2*t15+P(20,6)*t2*t10+P(20,2)*t2*t19-P(20,3)*t2*t22+P(20,5)*t2*t27);
            Kfusion[21] = t78*(P(21,0)*t2*t5-P(21,4)*t2*t7+P(21,1)*t2*t15+P(21,6)*t2*t10+P(21,2)*t2*t19-P(21,3)*t2*t22+P(21,5)*t2*t27);
            Kfusion[22] = t78*(P(22,0)*t2*t5-P(22,4)*t2*t7+P(22,1)*t2*t15+P(22,6)*t2*t10+P(22,2)*t2*t19-P(22,3)*t2*t22+P(22,5)*t2*t27);
            Kfusion[23] = t78*(P(23,0)*t2*t5-P(23,4)*t2*t7+P(23,1)*t2*t15+P(23,6)*t2*t10+P(23,2)*t2*t19-P(23,3)*t2*t22+P(23,5)*t2*t27);

            for (int row=0; row<24; row++) {
                Hfusion_matlab(row) = H_LOS[row];
                Kfusion_matlab(row) = Kfusion[row];
            }
        }

        // find largest observation variance difference as a fraction of the matlab value
        float max_diff_fraction = 0.0f;
        int max_row;
        float max_old, max_new;
        for (int row=0; row<24; row++) {
            float diff_fraction;
            if (Hfusion_matlab(row) != 0.0f) {
                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
            } else if (Hfusion_sympy(row) != 0.0f) {
                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
            } else {
                diff_fraction = 0.0f;
            }
            if (diff_fraction > max_diff_fraction) {
                max_diff_fraction = diff_fraction;
                max_row = row;
                max_old = Hfusion_matlab(row);
                max_new = Hfusion_sympy(row);
            }
        }

        if (max_diff_fraction > 1e-5f) {
            printf("Fail: Optical Flow X axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
        } else {
            printf("Pass: Optical Flow X axis Hfusion max diff fraction = %e\n",max_diff_fraction);
        }

        // find largest Kalman gain difference as a fraction of the matlab value
        max_diff_fraction = 0.0f;
        for (int row=0; row<24; row++) {
            float diff_fraction;
            if (Kfusion_matlab(row) != 0.0f) {
                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
            } else if (Hfusion_sympy(row) != 0.0f) {
                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
            } else {
                diff_fraction = 0.0f;
            }
            if (diff_fraction > max_diff_fraction) {
                max_diff_fraction = diff_fraction;
                max_row = row;
                max_old = Kfusion_matlab(row);
                max_new = Kfusion_sympy(row);
            }
        }

        if (max_diff_fraction > 1e-5f) {
            printf("Fail: Optical Flow X axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
        } else {
            printf("Pass: Optical Flow X axis Kfusion max diff fraction = %e\n",max_diff_fraction);
        }

    }

    // Compare Y axis equations

    {
        // evaluate sympy genrated equations for observatio Jacobians and Kalman gains
        {
            // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
            flow_innov_var = (HK43*HK74*HK90 + HK43*HK77*HK89 + HK43*HK79*HK88 + HK43*HK80*HK87 + HK66*HK92*HK94 + HK68*HK91*HK92 + HK70*HK92*HK93 + R_LOS);

            const float HK95 = HK4/flow_innov_var;

			// Observation Jacobians - axis 1
			Hfusion.at<0>() = -HK5*HK63;
			Hfusion.at<1>() = -HK5*HK66;
			Hfusion.at<2>() = -HK5*HK68;
			Hfusion.at<3>() = -HK5*HK70;
			Hfusion.at<4>() = -HK4*HK74;
			Hfusion.at<5>() = -HK4*HK77;
			Hfusion.at<6>() = -HK4*HK79;

			// Kalman gains - axis 1
			Kfusion(0) = -HK87*HK95;
			Kfusion(1) = -HK94*HK95;
			Kfusion(2) = -HK91*HK95;
			Kfusion(3) = -HK93*HK95;
			Kfusion(4) = -HK90*HK95;
			Kfusion(5) = -HK89*HK95;
			Kfusion(6) = -HK88*HK95;

			for (unsigned row = 7; row <= 23; row++) {
				Kfusion(row) = -HK95*(HK80*P(0,row) + HK81*P(1,row) + HK82*P(2,row) + HK83*P(3,row) + HK84*P(4,row) + HK85*P(5,row) + HK86*P(6,row));
			}

            // copy to arrays used for comparison
            for (int row=0; row<7; row++) {
                Hfusion_sympy(row) = Hfusion.atCompressedIndex(row);
            }
            for (int row=0; row<24; row++) {
                Kfusion_sympy(row) = Kfusion(row);
            }

        }

        // repeat calculation using matlab generated equations

        {
			// calculate Y axis observation Jacobian
			float t2 = 1.0f / range;
            float H_LOS[24] = {};
			H_LOS[0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
			H_LOS[1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
			H_LOS[2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
			H_LOS[3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
			H_LOS[4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
			H_LOS[5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
			H_LOS[6] = t2*(q0*q2*2.0f-q1*q3*2.0f);

			// calculate intermediate variables for the Y observation innovatoin variance and Kalman gains
			float t3 = q3*ve*2.0f;
			float t4 = q0*vn*2.0f;
			float t11 = q2*vd*2.0f;
			float t5 = t3+t4-t11;
			float t6 = q0*q3*2.0f;
			float t7 = q1*q2*2.0f;
			float t8 = t6+t7;
			float t9 = q0*q2*2.0f;
			float t28 = q1*q3*2.0f;
			float t10 = t9-t28;
			float t12 = P(0,0)*t2*t5;
			float t13 = q3*vd*2.0f;
			float t14 = q2*ve*2.0f;
			float t15 = q1*vn*2.0f;
			float t16 = t13+t14+t15;
			float t17 = q0*vd*2.0f;
			float t18 = q2*vn*2.0f;
			float t29 = q1*ve*2.0f;
			float t19 = t17+t18-t29;
			float t20 = q1*vd*2.0f;
			float t21 = q0*ve*2.0f;
			float t30 = q3*vn*2.0f;
			float t22 = t20+t21-t30;
			float t23 = q0*q0;
			float t24 = q1*q1;
			float t25 = q2*q2;
			float t26 = q3*q3;
			float t27 = t23+t24-t25-t26;
			float t31 = P(1,1)*t2*t16;
			float t32 = P(5,0)*t2*t8;
			float t33 = P(1,0)*t2*t16;
			float t34 = P(3,0)*t2*t22;
			float t35 = P(4,0)*t2*t27;
			float t80 = P(6,0)*t2*t10;
			float t81 = P(2,0)*t2*t19;
			float t36 = t12+t32+t33+t34+t35-t80-t81;
			float t37 = t2*t5*t36;
			float t38 = P(5,1)*t2*t8;
			float t39 = P(0,1)*t2*t5;
			float t40 = P(3,1)*t2*t22;
			float t41 = P(4,1)*t2*t27;
			float t82 = P(6,1)*t2*t10;
			float t83 = P(2,1)*t2*t19;
			float t42 = t31+t38+t39+t40+t41-t82-t83;
			float t43 = t2*t16*t42;
			float t44 = P(5,2)*t2*t8;
			float t45 = P(0,2)*t2*t5;
			float t46 = P(1,2)*t2*t16;
			float t47 = P(3,2)*t2*t22;
			float t48 = P(4,2)*t2*t27;
			float t79 = P(2,2)*t2*t19;
			float t84 = P(6,2)*t2*t10;
			float t49 = t44+t45+t46+t47+t48-t79-t84;
			float t50 = P(5,3)*t2*t8;
			float t51 = P(0,3)*t2*t5;
			float t52 = P(1,3)*t2*t16;
			float t53 = P(3,3)*t2*t22;
			float t54 = P(4,3)*t2*t27;
			float t86 = P(6,3)*t2*t10;
			float t87 = P(2,3)*t2*t19;
			float t55 = t50+t51+t52+t53+t54-t86-t87;
			float t56 = t2*t22*t55;
			float t57 = P(5,4)*t2*t8;
			float t58 = P(0,4)*t2*t5;
			float t59 = P(1,4)*t2*t16;
			float t60 = P(3,4)*t2*t22;
			float t61 = P(4,4)*t2*t27;
			float t88 = P(6,4)*t2*t10;
			float t89 = P(2,4)*t2*t19;
			float t62 = t57+t58+t59+t60+t61-t88-t89;
			float t63 = t2*t27*t62;
			float t64 = P(5,5)*t2*t8;
			float t65 = P(0,5)*t2*t5;
			float t66 = P(1,5)*t2*t16;
			float t67 = P(3,5)*t2*t22;
			float t68 = P(4,5)*t2*t27;
			float t90 = P(6,5)*t2*t10;
			float t91 = P(2,5)*t2*t19;
			float t69 = t64+t65+t66+t67+t68-t90-t91;
			float t70 = t2*t8*t69;
			float t71 = P(5,6)*t2*t8;
			float t72 = P(0,6)*t2*t5;
			float t73 = P(1,6)*t2*t16;
			float t74 = P(3,6)*t2*t22;
			float t75 = P(4,6)*t2*t27;
			float t92 = P(6,6)*t2*t10;
			float t93 = P(2,6)*t2*t19;
			float t76 = t71+t72+t73+t74+t75-t92-t93;
			float t85 = t2*t19*t49;
			float t94 = t2*t10*t76;
			float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;

			float t78 = 1.0f / t77;
			flow_innov_var = t77;

			// calculate Kalman gains for Y-axis observation
            float Kfusion[24];
			Kfusion[0] = -t78*(t12+P(0,5)*t2*t8-P(0,6)*t2*t10+P(0,1)*t2*t16-P(0,2)*t2*t19+P(0,3)*t2*t22+P(0,4)*t2*t27);
			Kfusion[1] = -t78*(t31+P(1,0)*t2*t5+P(1,5)*t2*t8-P(1,6)*t2*t10-P(1,2)*t2*t19+P(1,3)*t2*t22+P(1,4)*t2*t27);
			Kfusion[2] = -t78*(-t79+P(2,0)*t2*t5+P(2,5)*t2*t8-P(2,6)*t2*t10+P(2,1)*t2*t16+P(2,3)*t2*t22+P(2,4)*t2*t27);
			Kfusion[3] = -t78*(t53+P(3,0)*t2*t5+P(3,5)*t2*t8-P(3,6)*t2*t10+P(3,1)*t2*t16-P(3,2)*t2*t19+P(3,4)*t2*t27);
			Kfusion[4] = -t78*(t61+P(4,0)*t2*t5+P(4,5)*t2*t8-P(4,6)*t2*t10+P(4,1)*t2*t16-P(4,2)*t2*t19+P(4,3)*t2*t22);
			Kfusion[5] = -t78*(t64+P(5,0)*t2*t5-P(5,6)*t2*t10+P(5,1)*t2*t16-P(5,2)*t2*t19+P(5,3)*t2*t22+P(5,4)*t2*t27);
			Kfusion[6] = -t78*(-t92+P(6,0)*t2*t5+P(6,5)*t2*t8+P(6,1)*t2*t16-P(6,2)*t2*t19+P(6,3)*t2*t22+P(6,4)*t2*t27);
			Kfusion[7] = -t78*(P(7,0)*t2*t5+P(7,5)*t2*t8-P(7,6)*t2*t10+P(7,1)*t2*t16-P(7,2)*t2*t19+P(7,3)*t2*t22+P(7,4)*t2*t27);
			Kfusion[8] = -t78*(P(8,0)*t2*t5+P(8,5)*t2*t8-P(8,6)*t2*t10+P(8,1)*t2*t16-P(8,2)*t2*t19+P(8,3)*t2*t22+P(8,4)*t2*t27);
			Kfusion[9] = -t78*(P(9,0)*t2*t5+P(9,5)*t2*t8-P(9,6)*t2*t10+P(9,1)*t2*t16-P(9,2)*t2*t19+P(9,3)*t2*t22+P(9,4)*t2*t27);
			Kfusion[10] = -t78*(P(10,0)*t2*t5+P(10,5)*t2*t8-P(10,6)*t2*t10+P(10,1)*t2*t16-P(10,2)*t2*t19+P(10,3)*t2*t22+P(10,4)*t2*t27);
			Kfusion[11] = -t78*(P(11,0)*t2*t5+P(11,5)*t2*t8-P(11,6)*t2*t10+P(11,1)*t2*t16-P(11,2)*t2*t19+P(11,3)*t2*t22+P(11,4)*t2*t27);
			Kfusion[12] = -t78*(P(12,0)*t2*t5+P(12,5)*t2*t8-P(12,6)*t2*t10+P(12,1)*t2*t16-P(12,2)*t2*t19+P(12,3)*t2*t22+P(12,4)*t2*t27);
			Kfusion[13] = -t78*(P(13,0)*t2*t5+P(13,5)*t2*t8-P(13,6)*t2*t10+P(13,1)*t2*t16-P(13,2)*t2*t19+P(13,3)*t2*t22+P(13,4)*t2*t27);
			Kfusion[14] = -t78*(P(14,0)*t2*t5+P(14,5)*t2*t8-P(14,6)*t2*t10+P(14,1)*t2*t16-P(14,2)*t2*t19+P(14,3)*t2*t22+P(14,4)*t2*t27);
			Kfusion[15] = -t78*(P(15,0)*t2*t5+P(15,5)*t2*t8-P(15,6)*t2*t10+P(15,1)*t2*t16-P(15,2)*t2*t19+P(15,3)*t2*t22+P(15,4)*t2*t27);
			Kfusion[16] = -t78*(P(16,0)*t2*t5+P(16,5)*t2*t8-P(16,6)*t2*t10+P(16,1)*t2*t16-P(16,2)*t2*t19+P(16,3)*t2*t22+P(16,4)*t2*t27);
			Kfusion[17] = -t78*(P(17,0)*t2*t5+P(17,5)*t2*t8-P(17,6)*t2*t10+P(17,1)*t2*t16-P(17,2)*t2*t19+P(17,3)*t2*t22+P(17,4)*t2*t27);
			Kfusion[18] = -t78*(P(18,0)*t2*t5+P(18,5)*t2*t8-P(18,6)*t2*t10+P(18,1)*t2*t16-P(18,2)*t2*t19+P(18,3)*t2*t22+P(18,4)*t2*t27);
			Kfusion[19] = -t78*(P(19,0)*t2*t5+P(19,5)*t2*t8-P(19,6)*t2*t10+P(19,1)*t2*t16-P(19,2)*t2*t19+P(19,3)*t2*t22+P(19,4)*t2*t27);
			Kfusion[20] = -t78*(P(20,0)*t2*t5+P(20,5)*t2*t8-P(20,6)*t2*t10+P(20,1)*t2*t16-P(20,2)*t2*t19+P(20,3)*t2*t22+P(20,4)*t2*t27);
			Kfusion[21] = -t78*(P(21,0)*t2*t5+P(21,5)*t2*t8-P(21,6)*t2*t10+P(21,1)*t2*t16-P(21,2)*t2*t19+P(21,3)*t2*t22+P(21,4)*t2*t27);
			Kfusion[22] = -t78*(P(22,0)*t2*t5+P(22,5)*t2*t8-P(22,6)*t2*t10+P(22,1)*t2*t16-P(22,2)*t2*t19+P(22,3)*t2*t22+P(22,4)*t2*t27);
			Kfusion[23] = -t78*(P(23,0)*t2*t5+P(23,5)*t2*t8-P(23,6)*t2*t10+P(23,1)*t2*t16-P(23,2)*t2*t19+P(23,3)*t2*t22+P(23,4)*t2*t27);

            for (int row=0; row<24; row++) {
                Hfusion_matlab(row) = H_LOS[row];
                Kfusion_matlab(row) = Kfusion[row];
            }
        }

        // find largest observation variance difference as a fraction of the matlab value
        float max_diff_fraction = 0.0f;
        int max_row;
        float max_old, max_new;
        for (int row=0; row<24; row++) {
            float diff_fraction;
            if (Hfusion_matlab(row) != 0.0f) {
                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_matlab(row));
            } else if (Hfusion_sympy(row) != 0.0f) {
                diff_fraction = fabsf(Hfusion_sympy(row) - Hfusion_matlab(row)) / fabsf(Hfusion_sympy(row));
            } else {
                diff_fraction = 0.0f;
            }
            if (diff_fraction > max_diff_fraction) {
                max_diff_fraction = diff_fraction;
                max_row = row;
                max_old = Hfusion_matlab(row);
                max_new = Hfusion_sympy(row);
            }
        }

        if (max_diff_fraction > 1e-5f) {
            printf("Fail: Optical Flow Y axis Hfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
        } else {
            printf("Pass: Optical Flow Y axis Hfusion max diff fraction = %e\n",max_diff_fraction);
        }

        // find largest Kalman gain difference as a fraction of the matlab value
        max_diff_fraction = 0.0f;
        for (int row=0; row<24; row++) {
            float diff_fraction;
            if (Kfusion_matlab(row) != 0.0f) {
                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_matlab(row));
            } else if (Hfusion_sympy(row) != 0.0f) {
                diff_fraction = fabsf(Kfusion_sympy(row) - Kfusion_matlab(row)) / fabsf(Kfusion_sympy(row));
            } else {
                diff_fraction = 0.0f;
            }
            if (diff_fraction > max_diff_fraction) {
                max_diff_fraction = diff_fraction;
                max_row = row;
                max_old = Kfusion_matlab(row);
                max_new = Kfusion_sympy(row);
            }
        }

        if (max_diff_fraction > 1e-5f) {
            printf("Fail: Optical Flow Y axis Kfusion max diff fraction = %e , old = %e , new = %e , location index = %i\n",max_diff_fraction, max_old, max_new, max_row);
        } else {
            printf("Pass: Optical Flow Y axis Kfusion max diff fraction = %e\n",max_diff_fraction);
        }

    }

    return 0;
}