├── .gitattributes ├── Debug ├── FCF_test.exe ├── FCF_test.ilk └── FCF_test.pdb ├── FCF_test.sdf ├── FCF_test.sln ├── FCF_test.v12.suo ├── FCF_test ├── Debug │ ├── FCF_test.log │ ├── FCF_test.obj │ ├── FCF_test.pch │ ├── FCF_test.tlog │ │ ├── CL.read.1.tlog │ │ ├── CL.write.1.tlog │ │ ├── FCF_test.lastbuildstate │ │ ├── cl.command.1.tlog │ │ ├── link.command.1.tlog │ │ ├── link.read.1.tlog │ │ └── link.write.1.tlog │ ├── stdafx.obj │ ├── vc120.idb │ └── vc120.pdb ├── FCF_test.cpp ├── FCF_test.vcxproj ├── FCF_test.vcxproj.filters ├── ReadMe.txt ├── Release │ ├── FCF_test.log │ ├── FCF_test.obj │ ├── FCF_test.pch │ ├── FCF_test.tlog │ │ ├── CL.read.1.tlog │ │ ├── CL.write.1.tlog │ │ ├── FCF_test.lastbuildstate │ │ ├── cl.command.1.tlog │ │ ├── link.command.1.tlog │ │ ├── link.read.1.tlog │ │ └── link.write.1.tlog │ ├── stdafx.obj │ └── vc120.pdb ├── draw_curve.m ├── flat_motion2.txt ├── output.txt ├── quaternion_library │ ├── TestScript.m │ ├── axisAngle2quatern.m │ ├── axisAngle2rotMat.m │ ├── euler2rotMat.m │ ├── quatern2euler.m │ ├── quatern2rotMat.m │ ├── quaternConj.m │ ├── quaternProd.m │ ├── rotMat2euler.m │ └── rotMat2quatern.m ├── stdafx.cpp ├── stdafx.h └── targetver.h ├── README.md ├── Release ├── FCF_test.exe └── FCF_test.pdb └── ipch └── fcf_test-bde90cda ├── fcf_test-4eabdfa.ipch └── fcf_test-5bf45934.ipch /.gitattributes: -------------------------------------------------------------------------------- 1 | *.cpp linguist-language=C++ 2 | *.m linguist-language=C++ 3 | *.h linguist-language=C++ -------------------------------------------------------------------------------- /Debug/FCF_test.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/Debug/FCF_test.exe -------------------------------------------------------------------------------- /Debug/FCF_test.ilk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/Debug/FCF_test.ilk -------------------------------------------------------------------------------- /Debug/FCF_test.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/Debug/FCF_test.pdb -------------------------------------------------------------------------------- /FCF_test.sdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test.sdf -------------------------------------------------------------------------------- /FCF_test.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2013 4 | VisualStudioVersion = 12.0.40629.0 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "FCF_test", "FCF_test\FCF_test.vcxproj", "{BF6C1AB4-3229-4C34-816F-61A3632E9020}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|Win32 = Debug|Win32 11 | Release|Win32 = Release|Win32 12 | EndGlobalSection 13 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 14 | {BF6C1AB4-3229-4C34-816F-61A3632E9020}.Debug|Win32.ActiveCfg = Debug|Win32 15 | {BF6C1AB4-3229-4C34-816F-61A3632E9020}.Debug|Win32.Build.0 = Debug|Win32 16 | {BF6C1AB4-3229-4C34-816F-61A3632E9020}.Release|Win32.ActiveCfg = Release|Win32 17 | {BF6C1AB4-3229-4C34-816F-61A3632E9020}.Release|Win32.Build.0 = Release|Win32 18 | EndGlobalSection 19 | GlobalSection(SolutionProperties) = preSolution 20 | HideSolutionNode = FALSE 21 | EndGlobalSection 22 | EndGlobal 23 | -------------------------------------------------------------------------------- /FCF_test.v12.suo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test.v12.suo -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.log: -------------------------------------------------------------------------------- 1 | Build started 2016/8/5 23:58:36. 2 | 1>Project "c:\Users\zarathustra\documents\visual studio 2013\Projects\FCF_test\FCF_test\FCF_test.vcxproj" on node 2 (Build target(s)). 3 | 1>ClCompile: 4 | C:\Program Files (x86)\Microsoft Visual Studio 12.0\VC\bin\CL.exe /c /ZI /nologo /W3 /WX- /sdl /Od /Oy- /D WIN32 /D _DEBUG /D _CONSOLE /D _LIB /D _UNICODE /D UNICODE /Gm /EHsc /RTC1 /MDd /GS /fp:precise /Zc:wchar_t /Zc:forScope /Yu"stdafx.h" /Fp"Debug\FCF_test.pch" /Fo"Debug\\" /Fd"Debug\vc120.pdb" /Gd /TP /analyze- /errorReport:prompt FCF_test.cpp 5 | FCF_test.cpp 6 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(77): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 7 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(78): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 8 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(79): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 9 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(80): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 10 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(117): warning C4244: '+=' : conversion from 'double' to 'float', possible loss of data 11 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(119): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 12 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(123): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 13 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(131): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 14 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(133): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 15 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(140): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 16 | 1>c:\users\zarathustra\documents\visual studio 2013\projects\fcf_test\fcf_test\fcf_test.cpp(142): warning C4244: '=' : conversion from 'double' to 'float', possible loss of data 17 | Link: 18 | C:\Program Files (x86)\Microsoft Visual Studio 12.0\VC\bin\link.exe /ERRORREPORT:PROMPT /OUT:"c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Debug\FCF_test.exe" /INCREMENTAL /NOLOGO kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /MANIFEST /MANIFESTUAC:"level='asInvoker' uiAccess='false'" /manifest:embed /DEBUG /PDB:"c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Debug\FCF_test.pdb" /SUBSYSTEM:CONSOLE /TLBID:1 /DYNAMICBASE /NXCOMPAT /IMPLIB:"c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Debug\FCF_test.lib" /MACHINE:X86 Debug\FCF_test.obj 19 | Debug\stdafx.obj 20 | FCF_test.vcxproj -> c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Debug\FCF_test.exe 21 | 1>Done Building Project "c:\Users\zarathustra\documents\visual studio 2013\Projects\FCF_test\FCF_test\FCF_test.vcxproj" (Build target(s)). 22 | 23 | Build succeeded. 24 | 25 | Time Elapsed 00:00:01.03 26 | -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.obj -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.pch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.pch -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/CL.read.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.tlog/CL.read.1.tlog -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/CL.write.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.tlog/CL.write.1.tlog -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/FCF_test.lastbuildstate: -------------------------------------------------------------------------------- 1 | #TargetFrameworkVersion=v4.0:PlatformToolSet=v120:EnableManagedIncrementalBuild=false:VCToolArchitecture=Native32Bit 2 | Debug|Win32|c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\| 3 | -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/cl.command.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.tlog/cl.command.1.tlog -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/link.command.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.tlog/link.command.1.tlog -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/link.read.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.tlog/link.read.1.tlog -------------------------------------------------------------------------------- /FCF_test/Debug/FCF_test.tlog/link.write.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/FCF_test.tlog/link.write.1.tlog -------------------------------------------------------------------------------- /FCF_test/Debug/stdafx.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/stdafx.obj -------------------------------------------------------------------------------- /FCF_test/Debug/vc120.idb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/vc120.idb -------------------------------------------------------------------------------- /FCF_test/Debug/vc120.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Debug/vc120.pdb -------------------------------------------------------------------------------- /FCF_test/FCF_test.cpp: -------------------------------------------------------------------------------- 1 | // FCF_test.cpp : Defines the entry point for the console application. 2 | // 3 | 4 | #include "stdafx.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | std::ifstream input1("flat_motion2.txt"); 11 | std::ifstream input2("flat_motion2.txt"); 12 | std::ofstream output("output.txt"); 13 | 14 | 15 | float invSqrt(float x) 16 | { 17 | float xhalf = 0.5f * x; 18 | int i = *(int*)&x; // get bits for floating value 19 | i = 0x5f375a86 - (i >> 1); // gives initial guess 20 | x = *(float*)&i; // convert bits back to float 21 | x = x * (1.5f - xhalf*x*x); // Newton step 22 | return x; 23 | } 24 | 25 | 26 | void acc_mag(const float M[6], const float q_a[4], float q[4]) 27 | { 28 | int k; 29 | static const signed char iv0[4] = { 1, 0, 0, 0 }; 30 | 31 | float ab_idx_0; 32 | float ab_idx_1; 33 | float ab_idx_2; 34 | float ab_idx_3; 35 | float h_idx_1; 36 | float h_idx_2; 37 | float h_idx_3; 38 | float mn; 39 | float b1xr1[3]; 40 | float b2xr2[3]; 41 | float b_M[3]; 42 | float bxrx; 43 | float c_M[3]; 44 | float bxxrx[3]; 45 | float bx_rx[3]; 46 | float b_bxxrx; 47 | float alpha; 48 | float beta; 49 | float b_gamma; 50 | float norma; 51 | for (k = 0; k < 4; k++) { 52 | q[k] = iv0[k]; 53 | } 54 | 55 | /* QUATERN2ROTMAT Converts a quaternion to its conjugate */ 56 | /* */ 57 | /* qConj = quaternConj(q) */ 58 | /* */ 59 | /* Converts a quaternion to its conjugate. */ 60 | /* */ 61 | /* For more information see: */ 62 | /* http://www.x-io.co.uk/node/8#quaternions */ 63 | /* */ 64 | /* Date Author Notes */ 65 | /* 27/09/2011 SOH Madgwick Initial release */ 66 | /* QUATERNPROD Calculates the quaternion product */ 67 | /* */ 68 | /* ab = quaternProd(a, b) */ 69 | /* */ 70 | /* Calculates the quaternion product of quaternion a and b. */ 71 | /* */ 72 | /* For more information see: */ 73 | /* http://www.x-io.co.uk/node/8#quaternions */ 74 | /* */ 75 | /* Date Author Notes */ 76 | /* 27/09/2011 SOH Madgwick Initial release */ 77 | ab_idx_0 = ((0.0f * q_a[0] - M[3] * -q_a[1]) - M[4] * -q_a[2]) - M[5] * -q_a[3]; 78 | ab_idx_1 = ((0.0f * -q_a[1] + M[3] * q_a[0]) + M[4] * -q_a[3]) - M[5] * -q_a[2]; 79 | ab_idx_2 = ((0.0f * -q_a[2] - M[3] * -q_a[3]) + M[4] * q_a[0]) + M[5] * -q_a[1]; 80 | ab_idx_3 = ((0.0f * -q_a[3] + M[3] * -q_a[2]) - M[4] * -q_a[1]) + M[5] * q_a[0]; 81 | 82 | /* QUATERNPROD Calculates the quaternion product */ 83 | /* */ 84 | /* ab = quaternProd(a, b) */ 85 | /* */ 86 | /* Calculates the quaternion product of quaternion a and b. */ 87 | /* */ 88 | /* For more information see: */ 89 | /* http://www.x-io.co.uk/node/8#quaternions */ 90 | /* */ 91 | /* Date Author Notes */ 92 | /* 27/09/2011 SOH Madgwick Initial release */ 93 | h_idx_1 = ((q_a[0] * ab_idx_1 + q_a[1] * ab_idx_0) + q_a[2] * ab_idx_3) - q_a 94 | [3] * ab_idx_2; 95 | h_idx_2 = ((q_a[0] * ab_idx_2 - q_a[1] * ab_idx_3) + q_a[2] * ab_idx_0) + q_a 96 | [3] * ab_idx_1; 97 | h_idx_3 = ((q_a[0] * ab_idx_3 + q_a[1] * ab_idx_2) - q_a[2] * ab_idx_1) + q_a 98 | [3] * ab_idx_0; 99 | mn = sqrt(h_idx_1 * h_idx_1 + h_idx_2 * h_idx_2); 100 | b1xr1[0] = M[1]; 101 | b1xr1[1] = -M[0]; 102 | b1xr1[2] = 0.0f; 103 | b2xr2[0] = M[4] * h_idx_3; 104 | b2xr2[1] = -M[3] * h_idx_3 + M[5] * mn; 105 | b2xr2[2] = -M[4] * mn; 106 | b_M[0] = M[1] * M[3] - M[0] * M[4]; 107 | b_M[1] = 0.0f; 108 | b_M[2] = -M[2] * M[4] + M[1] * M[5]; 109 | bxrx = (M[2] * M[3] - M[0] * M[5]) / mn; 110 | c_M[0] = -M[2] * M[4] + M[1] * M[5]; 111 | c_M[1] = (M[2] * M[3] - M[0] * M[5]) + mn; 112 | c_M[2] = -M[1] * M[3] + M[0] * M[4]; 113 | ab_idx_0 = 0.0f; 114 | for (k = 0; k < 3; k++) { 115 | b_bxxrx = b_M[k] / mn; 116 | bx_rx[k] = c_M[k] / mn; 117 | ab_idx_0 += b_bxxrx * (0.5f * b1xr1[k] + 0.5f * b2xr2[k]); 118 | bxxrx[k] = b_bxxrx; 119 | b1xr1[k] = 0.5f * b1xr1[k] + 0.5f * b2xr2[k]; 120 | } 121 | 122 | alpha = (1.0f + bxrx) * (0.5f * M[2] + 0.5f * (M[5] * h_idx_3 + M[3] * mn)) + 123 | ab_idx_0; 124 | beta = 0.0f; 125 | for (k = 0; k < 3; k++) { 126 | beta += bx_rx[k] * b1xr1[k]; 127 | } 128 | 129 | b_gamma = sqrt(alpha * alpha + beta * beta); 130 | if (alpha >= 0.0) { 131 | norma = 2.0f / invSqrt(b_gamma * (b_gamma + alpha) * (1.0f + bxrx)); 132 | ab_idx_0 = b_gamma + alpha; 133 | q[0] = (b_gamma + alpha) * (1.0f + bxrx) / norma; 134 | for (k = 0; k < 3; k++) { 135 | q[k + 1] = (ab_idx_0 * bxxrx[k] + beta * bx_rx[k]) / norma; 136 | } 137 | } 138 | 139 | if (alpha < 0.0) { 140 | norma = 2.0f / invSqrt(b_gamma * (b_gamma - alpha) * (1.0f + bxrx)); 141 | ab_idx_0 = b_gamma - alpha; 142 | q[0] = beta * (1.0f + bxrx) / norma; 143 | for (k = 0; k < 3; k++) { 144 | q[k + 1] = (beta * bxxrx[k] + ab_idx_0 * bx_rx[k]) / norma; 145 | } 146 | } 147 | } 148 | 149 | 150 | 151 | 152 | 153 | void FCF_gyro_acc_mag(float *Gyroscope, float *Accelerometer, float *Magnetometer, float *q_, 154 | float gain_a, float gain_m, float std_norm_m, float threshold_m, float dt,float *q_est) 155 | { 156 | 157 | float norm_a, norm_m,q_acc[4],q_gravity_mag[4],norm_q; 158 | 159 | norm_a = invSqrt(Accelerometer[0] * Accelerometer[0] + Accelerometer[1] * Accelerometer[1] + Accelerometer[2] * Accelerometer[2]); 160 | Accelerometer[0] *= norm_a; 161 | Accelerometer[1] *= norm_a; 162 | Accelerometer[2] *= norm_a; 163 | 164 | norm_m = invSqrt(Magnetometer[0] * Magnetometer[0] + Magnetometer[1] * Magnetometer[1] + Magnetometer[2] * Magnetometer[2]); 165 | Magnetometer[0] *= norm_m; 166 | Magnetometer[1] *= norm_m; 167 | Magnetometer[2] *= norm_m; 168 | 169 | float P[4][4] = { 170 | { Accelerometer[2] + 1.0f , Accelerometer[1] , -Accelerometer[0] , 0 }, 171 | { Accelerometer[1] , -Accelerometer[2] + 1.0f , 0 , Accelerometer[0] }, 172 | { -Accelerometer[0] , 0 , -Accelerometer[2] + 1.0f , Accelerometer[1] }, 173 | { 0 , Accelerometer[0] , Accelerometer[1] , Accelerometer[2] + 1.0f } 174 | }; 175 | 176 | float omega[4][4] = { 177 | { 0 , -Gyroscope[0] , -Gyroscope[1] , -Gyroscope[2] }, 178 | { Gyroscope[0] , 0 , Gyroscope[2] , -Gyroscope[1] }, 179 | { Gyroscope[1] , -Gyroscope[2] , 0 , Gyroscope[0] }, 180 | { Gyroscope[2] , Gyroscope[1] , -Gyroscope[0] , 0 } 181 | }; 182 | 183 | if (fabs(1.0f/norm_a - 1.0f) < 0.2f) 184 | { 185 | 186 | float q_0[4], q_1[4]; 187 | 188 | q_0[0] = (0.5f*dt*omega[0][0] + 1.0f) * q_[0] + (0.5f*dt*omega[0][1]) * q_[1] + (0.5f*dt*omega[0][2]) * q_[2] + (0.5f*dt*omega[0][3]) * q_[3]; 189 | q_0[1] = (0.5f*dt*omega[1][0]) * q_[0] + (0.5f*dt*omega[1][1] + 1.0f) * q_[1] + (0.5f*dt*omega[1][2]) * q_[2] + (0.5f*dt*omega[1][3]) * q_[3]; 190 | q_0[2] = (0.5f*dt*omega[2][0]) * q_[0] + (0.5f*dt*omega[2][1]) * q_[1] + (0.5f*dt*omega[2][2] + 1.0f) * q_[2] + (0.5f*dt*omega[2][3]) * q_[3]; 191 | q_0[3] = (0.5f*dt*omega[3][0]) * q_[0] + (0.5f*dt*omega[3][1]) * q_[1] + (0.5f*dt*omega[3][2]) * q_[2] + (0.5f*dt*omega[3][3] + 1.0f) * q_[3]; 192 | 193 | q_0[0] *= (1.0f - gain_a); 194 | q_0[1] *= (1.0f - gain_a); 195 | q_0[2] *= (1.0f - gain_a); 196 | q_0[3] *= (1.0f - gain_a); 197 | 198 | q_1[0] = gain_a*0.5f*P[0][0] * q_[0] + gain_a*0.5f*P[0][1] * q_[1] + gain_a*0.5f*P[0][2] * q_[2] + gain_a*0.5f*P[0][3] * q_[3]; 199 | q_1[1] = gain_a*0.5f*P[1][0] * q_[0] + gain_a*0.5f*P[1][1] * q_[1] + gain_a*0.5f*P[1][2] * q_[2] + gain_a*0.5f*P[1][3] * q_[3]; 200 | q_1[2] = gain_a*0.5f*P[2][0] * q_[0] + gain_a*0.5f*P[2][1] * q_[1] + gain_a*0.5f*P[2][2] * q_[2] + gain_a*0.5f*P[2][3] * q_[3]; 201 | q_1[3] = gain_a*0.5f*P[3][0] * q_[0] + gain_a*0.5f*P[3][1] * q_[1] + gain_a*0.5f*P[3][2] * q_[2] + gain_a*0.5f*P[3][3] * q_[3]; 202 | 203 | 204 | q_acc[0] = q_0[0] + q_1[0]; 205 | q_acc[1] = q_0[1] + q_1[1]; 206 | q_acc[2] = q_0[2] + q_1[2]; 207 | q_acc[3] = q_0[3] + q_1[3]; 208 | 209 | } 210 | else 211 | { 212 | q_acc[0] = (0.5f*dt*omega[0][0] + 1.0f) * q_[0] + (0.5f*dt*omega[0][1]) * q_[1] + (0.5f*dt*omega[0][2]) * q_[2] + (0.5f*dt*omega[0][3]) * q_[3]; 213 | q_acc[1] = (0.5f*dt*omega[1][0]) * q_[0] + (0.5f*dt*omega[1][1] + 1.0f) * q_[1] + (0.5f*dt*omega[1][2]) * q_[2] + (0.5f*dt*omega[1][3]) * q_[3]; 214 | q_acc[2] = (0.5f*dt*omega[2][0]) * q_[0] + (0.5f*dt*omega[2][1]) * q_[1] + (0.5f*dt*omega[2][2] + 1.0f) * q_[2] + (0.5f*dt*omega[2][3]) * q_[3]; 215 | q_acc[3] = (0.5f*dt*omega[3][0]) * q_[0] + (0.5f*dt*omega[3][1]) * q_[1] + (0.5f*dt*omega[3][2]) * q_[2] + (0.5f*dt*omega[3][3] + 1.0f) * q_[3]; 216 | } 217 | 218 | norm_q = invSqrt(q_acc[0] * q_acc[0] + q_acc[1] * q_acc[1] + q_acc[2] * q_acc[2] + q_acc[3] * q_acc[3]); 219 | q_acc[0] *= norm_q; 220 | q_acc[1] *= norm_q; 221 | q_acc[2] *= norm_q; 222 | q_acc[3] *= norm_q; 223 | 224 | float vx, vy, vz; 225 | vx = 2.0f * (q_acc[1] * q_acc[3] - q_acc[0] * q_acc[2]); 226 | vy = 2.0f * (q_acc[0] * q_acc[1] + q_acc[2] * q_acc[3]); 227 | vz = 2.0f * (0.5f - q_acc[1]*q_acc[1] - q_acc[2]*q_acc[2]); 228 | 229 | float M[6] = { vx, vy, vz, Magnetometer[0], Magnetometer[1], Magnetometer[2] }; 230 | 231 | if (fabs(1.0f/norm_m - std_norm_m) < threshold_m) 232 | { 233 | acc_mag(M, q_acc, q_gravity_mag); 234 | 235 | q_est[0] = (1.0f - gain_m)*q_acc[0] + gain_m*q_gravity_mag[0]; 236 | q_est[1] = (1.0f - gain_m)*q_acc[1] + gain_m*q_gravity_mag[1]; 237 | q_est[2] = (1.0f - gain_m)*q_acc[2] + gain_m*q_gravity_mag[2]; 238 | q_est[3] = (1.0f - gain_m)*q_acc[3] + gain_m*q_gravity_mag[3]; 239 | 240 | norm_q = invSqrt(q_est[0] * q_est[0] + q_est[1] * q_est[1] + q_est[2] * q_est[2] + q_est[3] * q_est[3]); 241 | q_est[0] *= norm_q; 242 | q_est[1] *= norm_q; 243 | q_est[2] *= norm_q; 244 | q_est[3] *= norm_q; 245 | } 246 | else 247 | { 248 | q_est[0] = q_acc[0]; 249 | q_est[1] = q_acc[1]; 250 | q_est[2] = q_acc[2]; 251 | q_est[3] = q_acc[3]; 252 | } 253 | 254 | 255 | 256 | 257 | 258 | 259 | return; 260 | 261 | } 262 | 263 | 264 | int main() 265 | { 266 | 267 | float acc[3], gyro[3], mag[3], euler_true[3],time,dt,last_q[4],q_est[4]; 268 | 269 | q_est[0] = 1.0f; 270 | q_est[1] = 0.0f; 271 | q_est[2] = 0.0f; 272 | q_est[3] = 0.0f; 273 | 274 | last_q[0] = 1.0f; 275 | last_q[1] = 0.0f; 276 | last_q[2] = 0.0f; 277 | last_q[3] = 0.0f; 278 | 279 | dt = 1.0f / 500.0f; 280 | 281 | long length = 0; 282 | std::string in; 283 | 284 | while (std::getline(input1, in)) 285 | ++length; 286 | 287 | for (int i = 0; i < length; ++i) 288 | { 289 | input2 >> euler_true[0] >> euler_true[1] >> euler_true[2] 290 | >> acc[0] >> acc[1] >> acc[2] 291 | >> gyro[0] >> gyro[1] >> gyro[2] 292 | >> mag[0] >> mag[1] >> mag[2] >>dt; 293 | 294 | acc[0] *= -1.0f; 295 | acc[2] *= -1.0f; 296 | 297 | gyro[0] *= -1.0f; 298 | gyro[2] *= -1.0f; 299 | 300 | mag[0] *= -1.0f; 301 | mag[2] *= -1.0f; 302 | 303 | 304 | FCF_gyro_acc_mag(gyro, acc, mag, last_q, 0.005f, 0.01f, 0.285f, 0.5f, dt, q_est); 305 | 306 | last_q[0] = q_est[0]; 307 | last_q[1] = q_est[1]; 308 | last_q[2] = q_est[2]; 309 | last_q[3] = q_est[3]; 310 | 311 | output << q_est[0]<<" "<< q_est[1]<<" " << q_est[2]<<" " << q_est[3] << std::endl; 312 | 313 | 314 | 315 | } 316 | 317 | return(0); 318 | } 319 | 320 | 321 | 322 | -------------------------------------------------------------------------------- /FCF_test/FCF_test.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | 14 | {BF6C1AB4-3229-4C34-816F-61A3632E9020} 15 | Win32Proj 16 | FCF_test 17 | 18 | 19 | 20 | Application 21 | true 22 | v120 23 | Unicode 24 | 25 | 26 | Application 27 | false 28 | v120 29 | true 30 | Unicode 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | true 44 | 45 | 46 | false 47 | 48 | 49 | 50 | Use 51 | Level3 52 | Disabled 53 | WIN32;_DEBUG;_CONSOLE;_LIB;%(PreprocessorDefinitions) 54 | true 55 | 56 | 57 | Console 58 | true 59 | 60 | 61 | 62 | 63 | Level3 64 | Use 65 | MaxSpeed 66 | true 67 | true 68 | WIN32;NDEBUG;_CONSOLE;_LIB;%(PreprocessorDefinitions) 69 | true 70 | 71 | 72 | Console 73 | true 74 | true 75 | true 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | Create 89 | Create 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /FCF_test/FCF_test.vcxproj.filters: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hh;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | Header Files 23 | 24 | 25 | Header Files 26 | 27 | 28 | 29 | 30 | Source Files 31 | 32 | 33 | Source Files 34 | 35 | 36 | -------------------------------------------------------------------------------- /FCF_test/ReadMe.txt: -------------------------------------------------------------------------------- 1 | ======================================================================== 2 | CONSOLE APPLICATION : FCF_test Project Overview 3 | ======================================================================== 4 | 5 | AppWizard has created this FCF_test application for you. 6 | 7 | This file contains a summary of what you will find in each of the files that 8 | make up your FCF_test application. 9 | 10 | 11 | FCF_test.vcxproj 12 | This is the main project file for VC++ projects generated using an Application Wizard. 13 | It contains information about the version of Visual C++ that generated the file, and 14 | information about the platforms, configurations, and project features selected with the 15 | Application Wizard. 16 | 17 | FCF_test.vcxproj.filters 18 | This is the filters file for VC++ projects generated using an Application Wizard. 19 | It contains information about the association between the files in your project 20 | and the filters. This association is used in the IDE to show grouping of files with 21 | similar extensions under a specific node (for e.g. ".cpp" files are associated with the 22 | "Source Files" filter). 23 | 24 | FCF_test.cpp 25 | This is the main application source file. 26 | 27 | ///////////////////////////////////////////////////////////////////////////// 28 | Other standard files: 29 | 30 | StdAfx.h, StdAfx.cpp 31 | These files are used to build a precompiled header (PCH) file 32 | named FCF_test.pch and a precompiled types file named StdAfx.obj. 33 | 34 | ///////////////////////////////////////////////////////////////////////////// 35 | Other notes: 36 | 37 | AppWizard uses "TODO:" comments to indicate parts of the source code you 38 | should add to or customize. 39 | 40 | ///////////////////////////////////////////////////////////////////////////// 41 | -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.log: -------------------------------------------------------------------------------- 1 | Build started 2016/8/6 0:15:32. 2 | 1>Project "c:\Users\zarathustra\documents\visual studio 2013\Projects\FCF_test\FCF_test\FCF_test.vcxproj" on node 2 (Build target(s)). 3 | 1>ClCompile: 4 | C:\Program Files (x86)\Microsoft Visual Studio 12.0\VC\bin\CL.exe /c /Zi /nologo /W3 /WX- /sdl /O2 /Oi /Oy- /GL /D WIN32 /D NDEBUG /D _CONSOLE /D _LIB /D _UNICODE /D UNICODE /Gm- /EHsc /MD /GS /Gy /fp:precise /Zc:wchar_t /Zc:forScope /Yu"stdafx.h" /Fp"Release\FCF_test.pch" /Fo"Release\\" /Fd"Release\vc120.pdb" /Gd /TP /analyze- /errorReport:prompt FCF_test.cpp 5 | FCF_test.cpp 6 | 1>FCF_test.cpp(267): warning C4101: 'time' : unreferenced local variable 7 | Link: 8 | C:\Program Files (x86)\Microsoft Visual Studio 12.0\VC\bin\link.exe /ERRORREPORT:PROMPT /OUT:"c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Release\FCF_test.exe" /INCREMENTAL:NO /NOLOGO kernel32.lib user32.lib gdi32.lib winspool.lib comdlg32.lib advapi32.lib shell32.lib ole32.lib oleaut32.lib uuid.lib odbc32.lib odbccp32.lib /MANIFEST /MANIFESTUAC:"level='asInvoker' uiAccess='false'" /manifest:embed /DEBUG /PDB:"c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Release\FCF_test.pdb" /SUBSYSTEM:CONSOLE /OPT:REF /OPT:ICF /LTCG /TLBID:1 /DYNAMICBASE /NXCOMPAT /IMPLIB:"c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Release\FCF_test.lib" /MACHINE:X86 /SAFESEH Release\FCF_test.obj 9 | Release\stdafx.obj 10 | Generating code 11 | Finished generating code 12 | FCF_test.vcxproj -> c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\Release\FCF_test.exe 13 | 1>Done Building Project "c:\Users\zarathustra\documents\visual studio 2013\Projects\FCF_test\FCF_test\FCF_test.vcxproj" (Build target(s)). 14 | 15 | Build succeeded. 16 | 17 | Time Elapsed 00:00:00.85 18 | -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.obj -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.pch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.pch -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/CL.read.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.tlog/CL.read.1.tlog -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/CL.write.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.tlog/CL.write.1.tlog -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/FCF_test.lastbuildstate: -------------------------------------------------------------------------------- 1 | #TargetFrameworkVersion=v4.0:PlatformToolSet=v120:EnableManagedIncrementalBuild=false:VCToolArchitecture=Native32Bit 2 | Release|Win32|c:\users\zarathustra\documents\visual studio 2013\Projects\FCF_test\| 3 | -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/cl.command.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.tlog/cl.command.1.tlog -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/link.command.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.tlog/link.command.1.tlog -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/link.read.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.tlog/link.read.1.tlog -------------------------------------------------------------------------------- /FCF_test/Release/FCF_test.tlog/link.write.1.tlog: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/FCF_test.tlog/link.write.1.tlog -------------------------------------------------------------------------------- /FCF_test/Release/stdafx.obj: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/stdafx.obj -------------------------------------------------------------------------------- /FCF_test/Release/vc120.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/FCF_test/Release/vc120.pdb -------------------------------------------------------------------------------- /FCF_test/draw_curve.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | clc; 3 | 4 | addpath('quaternion_library'); 5 | 6 | conv=diag([-1 1 -1]); 7 | data=load('flat_motion2.txt'); 8 | quaternion_FCF=load('output.txt'); 9 | 10 | euler_FCF=quatern2euler(quaternConj(quaternion_FCF))*180/pi; 11 | 12 | time=1/500*(1:length(data(:,1))); 13 | 14 | figure(1); 15 | subplot(3,1,1); 16 | plot(time,-data(:,1),time,euler_FCF(:,1)); 17 | legend('Reference','Complementary'); 18 | xlabel('Time (s)'); 19 | ylabel('Roll (deg)'); 20 | 21 | subplot(3,1,2); 22 | plot(time,data(:,2),time,euler_FCF(:,2)); 23 | legend('Reference','Complementary'); 24 | xlabel('Time (s)'); 25 | ylabel('Pitch (deg)'); 26 | 27 | subplot(3,1,3); 28 | plot(time,-data(:,3)+180,time,euler_FCF(:,3)); 29 | legend('Reference','Complementary'); 30 | xlabel('Time (s)'); 31 | ylabel('Yaw (deg)'); -------------------------------------------------------------------------------- /FCF_test/quaternion_library/TestScript.m: -------------------------------------------------------------------------------- 1 | % TestScript.m 2 | % 3 | % This script tests the quaternion library functions to ensure that each 4 | % function output is consistent. 5 | % 6 | % Date Author Notes 7 | % 27/09/2011 SOH Madgwick Initial release 8 | 9 | %% Start of script 10 | 11 | close all; % close all figures 12 | clear; % clear all variables 13 | clc; % clear the command terminal 14 | 15 | %% Axis-angle to rotation matrix 16 | 17 | axis = [1 2 3]; 18 | axis = axis / norm(axis); 19 | angle = pi/2; 20 | 21 | R = axisAngle2rotMat(axis, angle); 22 | num = ' % 1.5f'; 23 | a = sprintf('\rAxis-angle to rotation matrix:'); 24 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:)); 25 | c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:)); 26 | d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:)); 27 | disp(strcat(a,b,c,d)); 28 | 29 | %% Axis-angle to quaternion 30 | 31 | q = axisAngle2quatern(axis, angle); 32 | num = ' % 1.5f'; 33 | a = sprintf('\rAxis-angle to quaternion:'); 34 | b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q); 35 | disp(strcat(a,b)); 36 | 37 | %% Quaternion to rotation matrix 38 | 39 | R = quatern2rotMat(q); 40 | num = ' % 1.5f'; 41 | a = sprintf('\rQuaternion to rotation matrix:'); 42 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:)); 43 | c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:)); 44 | d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:)); 45 | disp(strcat(a,b,c,d)); 46 | 47 | %% Rotation matrix to quaternion 48 | 49 | q = rotMat2quatern(R); 50 | num = ' % 1.5f'; 51 | a = sprintf('\rRotation matrix to quaternion:'); 52 | b = sprintf(strcat('\r', num, '\t', num, '\t', num, '\t', num), q); 53 | disp(strcat(a,b)); 54 | 55 | %% Rotation matrix to ZYX Euler angles 56 | 57 | euler = rotMat2euler(R); 58 | num = ' % 1.5f'; 59 | a = sprintf('\rRotation matrix to ZYX Euler angles:'); 60 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler); 61 | disp(strcat(a,b)); 62 | 63 | %% Quaternion to ZYX Euler angles 64 | 65 | euler = quatern2euler(q); 66 | num = ' % 1.5f'; 67 | a = sprintf('\rQuaternion to ZYX Euler angles:'); 68 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), euler); 69 | disp(strcat(a,b)); 70 | 71 | %% ZYX Euler angles to rotation matrix 72 | 73 | R = euler2rotMat(euler(1), euler(2), euler(3)); 74 | num = ' % 1.5f'; 75 | a = sprintf('\rZYX Euler angles to rotation matrix:'); 76 | b = sprintf(strcat('\r', num, '\t', num, '\t', num), R(1,:)); 77 | c = sprintf(strcat('\r', num, '\t', num, '\t', num), R(2,:)); 78 | d = sprintf(strcat('\r', num, '\t', num, '\t', num), R(3,:)); 79 | disp(strcat(a,b,c,d)); 80 | 81 | %% End of script -------------------------------------------------------------------------------- /FCF_test/quaternion_library/axisAngle2quatern.m: -------------------------------------------------------------------------------- 1 | function q = axisAngle2quatern(axis, angle) 2 | %AXISANGLE2QUATERN Converts an axis-angle orientation to a quaternion 3 | % 4 | % q = axisAngle2quatern(axis, angle) 5 | % 6 | % Converts and axis-angle orientation to a quaternion where a 3D rotation 7 | % is described by an angular rotation around axis defined by a vector. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | q0 = cos(angle./2); 16 | q1 = -axis(:,1)*sin(angle./2); 17 | q2 = -axis(:,2)*sin(angle./2); 18 | q3 = -axis(:,3)*sin(angle./2); 19 | q = [q0 q1 q2 q3]; 20 | end 21 | 22 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/axisAngle2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = axisAngle2rotMat(axis, angle) 2 | %AXISANGLE2ROTMAT Converts an axis-angle orientation to a rotation matrix 3 | % 4 | % q = axisAngle2rotMat(axis, angle) 5 | % 6 | % Converts and axis-angle orientation to a rotation matrix where a 3D 7 | % rotation is described by an angular rotation around axis defined by a 8 | % vector. 9 | % 10 | % For more information see: 11 | % http://www.x-io.co.uk/node/8#quaternions 12 | % 13 | % Date Author Notes 14 | % 27/09/2011 SOH Madgwick Initial release 15 | 16 | kx = axis(:,1); 17 | ky = axis(:,2); 18 | kz = axis(:,3); 19 | cT = cos(angle); 20 | sT = sin(angle); 21 | vT = 1 - cos(angle); 22 | 23 | R(1,1,:) = kx.*kx.*vT + cT; 24 | R(1,2,:) = kx.*ky.*vT - kz.*sT; 25 | R(1,3,:) = kx.*kz.*vT + ky.*sT; 26 | 27 | R(2,1,:) = kx.*ky.*vT + kz.*sT; 28 | R(2,2,:) = ky.*ky.*vT + cT; 29 | R(2,3,:) = ky.*kz.*vT - kx.*sT; 30 | 31 | R(3,1,:) = kx.*kz.*vT - ky.*sT; 32 | R(3,2,:) = ky.*kz.*vT + kx.*sT; 33 | R(3,3,:) = kz.*kz.*vT + cT; 34 | end 35 | 36 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/euler2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = euler2rotMat(phi, theta, psi) 2 | %EULER2ROTMAT Converts a ZYX Euler angle orientation to a rotation matrix 3 | % 4 | % q = euler2rotMat(axis, angle) 5 | % 6 | % Converts ZYX Euler angle orientation to a rotation matrix where phi is 7 | % a rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | R(1,1,:) = cos(psi).*cos(theta); 16 | R(1,2,:) = -sin(psi).*cos(phi) + cos(psi).*sin(theta).*sin(phi); 17 | R(1,3,:) = sin(psi).*sin(phi) + cos(psi).*sin(theta).*cos(phi); 18 | 19 | R(2,1,:) = sin(psi).*cos(theta); 20 | R(2,2,:) = cos(psi).*cos(phi) + sin(psi).*sin(theta).*sin(phi); 21 | R(2,3,:) = -cos(psi).*sin(phi) + sin(psi).*sin(theta).*cos(phi); 22 | 23 | R(3,1,:) = -sin(theta); 24 | R(3,2,:) = cos(theta).*sin(phi); 25 | R(3,3,:) = cos(theta).*cos(phi); 26 | end 27 | 28 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/quatern2euler.m: -------------------------------------------------------------------------------- 1 | function euler = quatern2euler(q) 2 | %QUATERN2EULER Converts a quaternion orientation to ZYX Euler angles 3 | % 4 | % q = quatern2euler(q) 5 | % 6 | % Converts a quaternion orientation to ZYX Euler angles where phi is a 7 | % rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2; 16 | R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4)); 17 | R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3)); 18 | R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2)); 19 | R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2; 20 | 21 | phi = atan2(R(3,2,:), R(3,3,:) ); 22 | theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) ); 23 | psi = atan2(R(2,1,:), R(1,1,:) ); 24 | 25 | euler = [phi(1,:)' theta(1,:)' psi(1,:)']; 26 | end 27 | 28 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/quatern2rotMat.m: -------------------------------------------------------------------------------- 1 | function R = quatern2rotMat(q) 2 | %QUATERN2ROTMAT Converts a quaternion orientation to a rotation matrix 3 | % 4 | % R = quatern2rotMat(q) 5 | % 6 | % Converts a quaternion orientation to a rotation matrix. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | R(1,1,:) = 2.*q(:,1).^2-1+2.*q(:,2).^2; 15 | R(1,2,:) = 2.*(q(:,2).*q(:,3)+q(:,1).*q(:,4)); 16 | R(1,3,:) = 2.*(q(:,2).*q(:,4)-q(:,1).*q(:,3)); 17 | R(2,1,:) = 2.*(q(:,2).*q(:,3)-q(:,1).*q(:,4)); 18 | R(2,2,:) = 2.*q(:,1).^2-1+2.*q(:,3).^2; 19 | R(2,3,:) = 2.*(q(:,3).*q(:,4)+q(:,1).*q(:,2)); 20 | R(3,1,:) = 2.*(q(:,2).*q(:,4)+q(:,1).*q(:,3)); 21 | R(3,2,:) = 2.*(q(:,3).*q(:,4)-q(:,1).*q(:,2)); 22 | R(3,3,:) = 2.*q(:,1).^2-1+2.*q(:,4).^2; 23 | end 24 | 25 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/quaternConj.m: -------------------------------------------------------------------------------- 1 | function qConj = quaternConj(q) 2 | %QUATERN2ROTMAT Converts a quaternion to its conjugate 3 | % 4 | % qConj = quaternConj(q) 5 | % 6 | % Converts a quaternion to its conjugate. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | qConj = [q(:,1) -q(:,2) -q(:,3) -q(:,4)]; 15 | end 16 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/quaternProd.m: -------------------------------------------------------------------------------- 1 | function ab = quaternProd(a, b) 2 | %QUATERNPROD Calculates the quaternion product 3 | % 4 | % ab = quaternProd(a, b) 5 | % 6 | % Calculates the quaternion product of quaternion a and b. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | ab=[1,0,0,0]; 14 | 15 | ab(1) = a(1).*b(1)-a(2).*b(2)-a(3).*b(3)-a(4).*b(4); 16 | ab(2) = a(1).*b(2)+a(2).*b(1)+a(3).*b(4)-a(4).*b(3); 17 | ab(3) = a(1).*b(3)-a(2).*b(4)+a(3).*b(1)+a(4).*b(2); 18 | ab(4) = a(1).*b(4)+a(2).*b(3)-a(3).*b(2)+a(4).*b(1); 19 | end 20 | 21 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/rotMat2euler.m: -------------------------------------------------------------------------------- 1 | function euler = rotMat2euler(R) 2 | %ROTMAT2EULER Converts a rotation matrix orientation to ZYX Euler angles 3 | % 4 | % euler = rotMat2euler(R) 5 | % 6 | % Converts a rotation matrix orientation to ZYX Euler angles where phi is 7 | % a rotation around X, theta around Y and psi around Z. 8 | % 9 | % For more information see: 10 | % http://www.x-io.co.uk/node/8#quaternions 11 | % 12 | % Date Author Notes 13 | % 27/09/2011 SOH Madgwick Initial release 14 | 15 | phi = atan2(R(3,2,:), R(3,3,:) ); 16 | theta = -atan(R(3,1,:) ./ sqrt(1-R(3,1,:).^2) ); 17 | psi = atan2(R(2,1,:), R(1,1,:) ); 18 | 19 | euler = [phi(1,:)' theta(1,:)' psi(1,:)']; 20 | end 21 | 22 | -------------------------------------------------------------------------------- /FCF_test/quaternion_library/rotMat2quatern.m: -------------------------------------------------------------------------------- 1 | function q = rotMat2quatern(R) 2 | %ROTMAT2QUATERN Converts a rotation matrix orientation to a quaternion 3 | % 4 | % q = axisAngle2quatern(axis, angle) 5 | % 6 | % Converts a rotation matrix orientation to a quaternion. 7 | % 8 | % For more information see: 9 | % http://www.x-io.co.uk/node/8#quaternions 10 | % 11 | % Date Author Notes 12 | % 27/09/2011 SOH Madgwick Initial release 13 | 14 | [row col numR] = size(R); 15 | q = zeros(numR, 4); 16 | K = zeros(4,4); 17 | for i = 1:numR 18 | K(1,1) = (1/3) * (R(1,1,i) - R(2,2,i) - R(3,3,i)); 19 | K(1,2) = (1/3) * (R(2,1,i) + R(1,2,i)); 20 | K(1,3) = (1/3) * (R(3,1,i) + R(1,3,i)); 21 | K(1,4) = (1/3) * (R(2,3,i) - R(3,2,i)); 22 | K(2,1) = (1/3) * (R(2,1,i) + R(1,2,i)); 23 | K(2,2) = (1/3) * (R(2,2,i) - R(1,1,i) - R(3,3,i)); 24 | K(2,3) = (1/3) * (R(3,2,i) + R(2,3,i)); 25 | K(2,4) = (1/3) * (R(3,1,i) - R(1,3,i)); 26 | K(3,1) = (1/3) * (R(3,1,i) + R(1,3,i)); 27 | K(3,2) = (1/3) * (R(3,2,i) + R(2,3,i)); 28 | K(3,3) = (1/3) * (R(3,3,i) - R(1,1,i) - R(2,2,i)); 29 | K(3,4) = (1/3) * (R(1,2,i) - R(2,1,i)); 30 | K(4,1) = (1/3) * (R(2,3,i) - R(3,2,i)); 31 | K(4,2) = (1/3) * (R(3,1,i) - R(1,3,i)); 32 | K(4,3) = (1/3) * (R(1,2,i) - R(2,1,i)); 33 | K(4,4) = (1/3) * (R(1,1,i) + R(2,2,i) + R(3,3,i)); 34 | [V,D] = eig(K); 35 | %p = find(max(D)); 36 | %q = V(:,p)'; 37 | q(i,:) = V(:,4)'; 38 | q(i,:) = [q(i,4) q(i,1) q(i,2) q(i,3)]; 39 | end 40 | end -------------------------------------------------------------------------------- /FCF_test/stdafx.cpp: -------------------------------------------------------------------------------- 1 | // stdafx.cpp : source file that includes just the standard includes 2 | // FCF_test.pch will be the pre-compiled header 3 | // stdafx.obj will contain the pre-compiled type information 4 | 5 | #include "stdafx.h" 6 | 7 | // TODO: reference any additional headers you need in STDAFX.H 8 | // and not in this file 9 | -------------------------------------------------------------------------------- /FCF_test/stdafx.h: -------------------------------------------------------------------------------- 1 | // stdafx.h : include file for standard system include files, 2 | // or project specific include files that are used frequently, but 3 | // are changed infrequently 4 | // 5 | 6 | #pragma once 7 | 8 | #include "targetver.h" 9 | 10 | #include 11 | #include 12 | 13 | 14 | 15 | // TODO: reference additional headers your program requires here 16 | -------------------------------------------------------------------------------- /FCF_test/targetver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Including SDKDDKVer.h defines the highest available Windows platform. 4 | 5 | // If you wish to build your application for a previous Windows platform, include WinSDKVer.h and 6 | // set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h. 7 | 8 | #include 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors 2 | This algorithm is proposed by Jin Wu, Zebo Zhou, Jingjun Chen, Hassen Fourati and Rui Li, which presents a novel fast approach for attitude estimation using gyroscope, accelerometer and magnetometer. Related Paper has been published as a regular paper on IEEE Sensors Jounral. Please download the PDF file at http://dx.doi.org/10.1109/JSEN.2016.2589660. 3 | 4 | The source codes are written with C++ programming language and are complied by Microsoft Visual Studio 2013 Comunity. 5 | 6 | Raw data file 'flat_motion2.txt' is included for algorithm's computation while the results are verified using MATLAB software with respect to gold reference angles acquired from an MicroStrain 3DM-GX3-25 Attitude and Heading Reference System (AHRS). 7 | 8 | This code can only be adopted for personal use. If any commercial use is involved, please contact: jin_wu_uestc@hotmail.com or klinsmann.zhou@gmail.com for further information. 9 | 10 | Anyone who uses the codes for academic research should refer to the following citation: "J. Wu, Z. Zhou, J. Chen, H. Fourati and R. Li, Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors, IEEE Sensors Journal, 2016". 11 | 12 | -------------------------------------------------------------------------------- /Release/FCF_test.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/Release/FCF_test.exe -------------------------------------------------------------------------------- /Release/FCF_test.pdb: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/Release/FCF_test.pdb -------------------------------------------------------------------------------- /ipch/fcf_test-bde90cda/fcf_test-4eabdfa.ipch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/ipch/fcf_test-bde90cda/fcf_test-4eabdfa.ipch -------------------------------------------------------------------------------- /ipch/fcf_test-bde90cda/fcf_test-5bf45934.ipch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zarathustr/Fast_Complementary_Filter/49986f09f629d37861610a2e8586df7ceceb2d16/ipch/fcf_test-bde90cda/fcf_test-5bf45934.ipch --------------------------------------------------------------------------------