├── ImuSample ├── .gitignore ├── Assets │ ├── Scenes.meta │ ├── Scenes │ │ ├── ImuTest.unity │ │ └── ImuTest.unity.meta │ ├── Scripts.meta │ ├── Scripts │ │ ├── GLWireframe.meta │ │ ├── GLWireframe │ │ │ ├── ColoredBlendedLine.shader │ │ │ ├── ColoredBlendedLine.shader.meta │ │ │ ├── GLDrawerBase.cs │ │ │ ├── GLDrawerBase.cs.meta │ │ │ ├── GLDrawerCamera.cs │ │ │ ├── GLDrawerCamera.cs.meta │ │ │ ├── GLGridDrawer.cs │ │ │ ├── GLGridDrawer.cs.meta │ │ │ ├── GLMicVisualiser.cs │ │ │ ├── GLMicVisualiser.cs.meta │ │ │ ├── GridMat.mat │ │ │ └── GridMat.mat.meta │ │ ├── IMU.meta │ │ ├── IMU │ │ │ ├── DVector3.cs │ │ │ ├── DVector3.cs.meta │ │ │ ├── IMUObject.cs │ │ │ └── IMUObject.cs.meta │ │ ├── Kalman.meta │ │ ├── MicAnalyzer.cs │ │ └── MicAnalyzer.cs.meta │ ├── Standard Assets.meta │ ├── Standard Assets │ │ ├── Skyboxes.meta │ │ └── Skyboxes │ │ │ ├── Sunny1 Skybox.mat │ │ │ ├── Sunny1 Skybox.mat.meta │ │ │ ├── Textures.meta │ │ │ └── Textures │ │ │ ├── Sunny1.meta │ │ │ └── Sunny1 │ │ │ ├── Sunny1_back.tif │ │ │ ├── Sunny1_back.tif.meta │ │ │ ├── Sunny1_down.tif │ │ │ ├── Sunny1_down.tif.meta │ │ │ ├── Sunny1_front.tif │ │ │ ├── Sunny1_front.tif.meta │ │ │ ├── Sunny1_left.tif │ │ │ ├── Sunny1_left.tif.meta │ │ │ ├── Sunny1_right.tif │ │ │ ├── Sunny1_right.tif.meta │ │ │ ├── Sunny1_up.tif │ │ │ └── Sunny1_up.tif.meta │ ├── TestShaders.unity │ └── TestShaders.unity.meta ├── ProjectSettings │ ├── AudioManager.asset │ ├── DynamicsManager.asset │ ├── EditorBuildSettings.asset │ ├── EditorSettings.asset │ ├── InputManager.asset │ ├── NavMeshLayers.asset │ ├── NetworkManager.asset │ ├── ProjectSettings.asset │ ├── QualitySettings.asset │ ├── TagManager.asset │ └── TimeManager.asset └── README.md ├── KalmanSample ├── .gitignore ├── Assets │ ├── Material.meta │ ├── Material │ │ ├── MouseColorGreen.mat │ │ ├── MouseColorGreen.mat.meta │ │ ├── MouseColorRed.mat │ │ ├── MouseColorRed.mat.meta │ │ ├── TrailGreen.mat │ │ ├── TrailGreen.mat.meta │ │ ├── TrailRed.mat │ │ └── TrailRed.mat.meta │ ├── Plugins.meta │ ├── Plugins │ │ ├── Kalman.meta │ │ └── Kalman │ │ │ ├── IKalmanWrapper.cs │ │ │ ├── IKalmanWrapper.cs.meta │ │ │ ├── Math.meta │ │ │ ├── Math │ │ │ ├── KalmanFilter.cs │ │ │ ├── KalmanFilter.cs.meta │ │ │ ├── KalmanFilterSimple1D.cs │ │ │ ├── KalmanFilterSimple1D.cs.meta │ │ │ ├── Matrix.cs │ │ │ └── Matrix.cs.meta │ │ │ ├── MatrixKalmanWrapper.cs │ │ │ ├── MatrixKalmanWrapper.cs.meta │ │ │ ├── SimpleKalmanWrapper.cs │ │ │ ├── SimpleKalmanWrapper.cs.meta │ │ │ ├── Simulate.meta │ │ │ └── Simulate │ │ │ ├── GuassianRandom.cs │ │ │ └── GuassianRandom.cs.meta │ ├── Scenes.meta │ ├── Scenes │ │ ├── Test.unity │ │ └── Test.unity.meta │ ├── Scripts.meta │ └── Scripts │ │ ├── Test.cs │ │ └── Test.cs.meta └── ProjectSettings │ ├── AudioManager.asset │ ├── DynamicsManager.asset │ ├── EditorBuildSettings.asset │ ├── EditorSettings.asset │ ├── InputManager.asset │ ├── NavMeshLayers.asset │ ├── NetworkManager.asset │ ├── ProjectSettings.asset │ ├── QualitySettings.asset │ ├── TagManager.asset │ └── TimeManager.asset └── README.md /ImuSample/.gitignore: -------------------------------------------------------------------------------- 1 | Library 2 | Temp 3 | *.csproj 4 | *.sln 5 | *.pidb 6 | *.userprefs 7 | *.apk 8 | ios 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scenes.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 72b2b5be1e508484ba043e6bf56ff88d 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scenes/ImuTest.unity: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Scenes/ImuTest.unity -------------------------------------------------------------------------------- /ImuSample/Assets/Scenes/ImuTest.unity.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 7264ec945b1ee42ec8515b4d2184368c 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 03d8c05d582234bd1964e2444726daba 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f91e6d936ccfd4ae2bbcc39c98056cbd 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/ColoredBlendedLine.shader: -------------------------------------------------------------------------------- 1 | Shader "Custom/ColoredBlendedLine" { 2 | SubShader { 3 | Pass { 4 | Blend SrcAlpha OneMinusSrcAlpha 5 | ZWrite Off Cull Off Fog { Mode Off } 6 | BindChannels { 7 | Bind "vertex", vertex Bind "color", color } 8 | } 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/ColoredBlendedLine.shader.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 51e8d44315fc2440a89d9657ab3ee900 3 | ShaderImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GLDrawerBase.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | 3 | [AddComponentMenu("Wireframe/GL Drawer Base")] 4 | public abstract class GLDrawerBase : MonoBehaviour { 5 | 6 | public void Draw (Matrix4x4 camMatrix) 7 | { 8 | if (!this.enabled) { 9 | return; 10 | } 11 | 12 | GL.PushMatrix (); 13 | GL.MultMatrix (camMatrix); 14 | GL.MultMatrix (transform.localToWorldMatrix); 15 | OnDraw (); 16 | GL.PopMatrix (); 17 | } 18 | 19 | /// 20 | /// override this method to Draw GL 21 | /// 22 | protected abstract void OnDraw (); 23 | } 24 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GLDrawerBase.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e665f4b7e6d684161b9b2351fa065606 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GLDrawerCamera.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | 3 | /// 4 | /// GL drawer camera. 5 | /// 6 | [RequireComponent(typeof(Camera))] 7 | [AddComponentMenu("Wireframe/GL Drawer Camera")] 8 | public class GLDrawerCamera : MonoBehaviour { 9 | 10 | [SerializeField] 11 | Material mat; 12 | 13 | [SerializeField] 14 | GLDrawerBase[] drawers; 15 | 16 | [SerializeField] 17 | int antiAliasing = 2; 18 | 19 | Camera _camera; 20 | 21 | void Start () 22 | { 23 | QualitySettings.antiAliasing = antiAliasing; 24 | 25 | // cache 26 | _camera = this.camera; 27 | } 28 | 29 | void OnPreRender () 30 | { 31 | GL.wireframe = true; 32 | } 33 | 34 | void OnPostRender () 35 | { 36 | GL.wireframe = false; 37 | 38 | GL.PushMatrix (); 39 | mat.SetPass (0); 40 | 41 | Matrix4x4 mtx = _camera.cameraToWorldMatrix; 42 | foreach (var drawer in drawers) { 43 | drawer.Draw (mtx); 44 | } 45 | 46 | GL.PopMatrix (); 47 | } 48 | } -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GLDrawerCamera.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: fff04dc30ff4f4467b1ac706e7fbf02c 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GLGridDrawer.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | 3 | [AddComponentMenu("Wireframe/GL Grid")] 4 | public class GLGridDrawer : GLDrawerBase { 5 | 6 | [SerializeField] 7 | int count; 8 | 9 | [SerializeField] 10 | float scale; 11 | 12 | [SerializeField] 13 | float distance; 14 | 15 | [SerializeField] 16 | Color color; 17 | 18 | void Start () 19 | { 20 | 21 | } 22 | 23 | protected override void OnDraw () 24 | { 25 | GL.Begin (GL.LINES); 26 | GL.Color (color); 27 | 28 | Vector3 pos; 29 | for (int x=0; x (); 21 | } 22 | 23 | void Update () 24 | { 25 | //string msg = string.Format ("RMS: {0:0.00} ({1:0.0} dB) Pitch: {2:0} Hz", analyzer.RMS, analyzer.DB, analyzer.Pitch); 26 | //Debug.Log (msg); 27 | } 28 | 29 | protected override void OnDraw () 30 | { 31 | float[] samples = analyzer.Samples; 32 | int i; 33 | float pi2 = Mathf.PI * 2; 34 | //float pos 35 | 36 | GL.Begin (GL.LINES); 37 | GL.Color (color); 38 | 39 | for (i=1; i<1024; i+=3) { 40 | rotPos (i / 1024.0f * pi2, Mathf.Pow(samples [i], 2)); 41 | } 42 | GL.End (); 43 | } 44 | 45 | void rotPos (float rot, float volume) 46 | { 47 | float x = Mathf.Cos (rot); 48 | float y = Mathf.Sin (rot); 49 | 50 | //Debug.Log (volume); 51 | float len = size - volume * amp; 52 | GL.Vertex3 (x * len, y * len, 0); 53 | len = size + volume * amp; 54 | GL.Vertex3 (x * len, y * len, 0); 55 | } 56 | } 57 | 58 | 59 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GLMicVisualiser.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d33b0d22014c04a088e3bc09bb6e5f6f 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GridMat.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Scripts/GLWireframe/GridMat.mat -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/GLWireframe/GridMat.mat.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 75cd3d15f23fd42ec9d387ab5d078773 3 | NativeFormatImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/IMU.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 09aa824bd6a5340ccb7cd23200eea9e3 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/IMU/DVector3.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | 3 | namespace Imu { 4 | 5 | /// 6 | /// Double Accuracy Vector3 class. 7 | /// 8 | public struct DVector3 { 9 | public double x; 10 | public double y; 11 | public double z; 12 | 13 | #region constractor 14 | public DVector3 (double xd, double yd, double zd) 15 | { 16 | x = xd; 17 | y = yd; 18 | z = zd; 19 | } 20 | 21 | public DVector3 (Vector3 vf) 22 | { 23 | x = vf.x; 24 | y = vf.y; 25 | z = vf.z; 26 | } 27 | 28 | public DVector3 (float xf, float yf, float zf) 29 | { 30 | x = xf; 31 | y = yf; 32 | z = zf; 33 | } 34 | #endregion 35 | 36 | #region public overrides 37 | 38 | // Indexer declaration 39 | public double this [int index] { 40 | get { 41 | if (index == 0) { 42 | return x; 43 | } else if (index == 1) { 44 | return y; 45 | } else if (index == 2) { 46 | return z; 47 | } else { 48 | throw new System.IndexOutOfRangeException (); 49 | } 50 | } 51 | set { 52 | if (index == 0) { 53 | x = value; 54 | } else if (index == 1) { 55 | y = value; 56 | } else if (index == 2) { 57 | z = value; 58 | } else { 59 | throw new System.IndexOutOfRangeException (); 60 | } 61 | } 62 | } 63 | 64 | public override string ToString () 65 | { 66 | return string.Format ("({0:0.000}, {1:0.000}, {2:0.000})", x, y, z); 67 | } 68 | 69 | 70 | public override bool Equals (object obj) 71 | { 72 | if (obj == null || this.GetType () != obj.GetType ()) { 73 | return false; 74 | } 75 | DVector3 v = (DVector3)obj; 76 | return this == v; 77 | } 78 | 79 | public override int GetHashCode () 80 | { 81 | //XOR 82 | return x.GetHashCode () ^ y.GetHashCode () ^ z.GetHashCode (); 83 | } 84 | #endregion 85 | 86 | #region pubic 87 | public Vector3 ToVector3 () 88 | { 89 | return new Vector3 ((float)x, (float)y, (float)z); 90 | } 91 | #endregion 92 | 93 | #region operator overrides 94 | /* 95 | * OVERRIDES 96 | * 97 | * operator + Adds two vectors. 98 | * operator - Subtracts one vector from another. 99 | * operator * Multiplies a vector by a number. 100 | * operator / Divides a vector by a number. 101 | * operator == Returns true if the vectors are equal. 102 | * operator != Returns true if vectors different. 103 | */ 104 | 105 | // + 106 | public static DVector3 operator+ (DVector3 a, DVector3 b) 107 | { 108 | return new DVector3 (a.x + b.x, a.y + b.y, a.z + b.z); 109 | } 110 | 111 | public static DVector3 operator+ (Vector3 a, DVector3 b) 112 | { 113 | return new DVector3 (a.x + b.x, a.y + b.y, a.z + b.z); 114 | } 115 | 116 | public static DVector3 operator+ (DVector3 a, Vector3 b) 117 | { 118 | return new DVector3 (a.x + b.x, a.y + b.y, a.z + b.z); 119 | } 120 | 121 | public static DVector3 operator+ (DVector3 v, double d) 122 | { 123 | return new DVector3 (v.x + d, v.y + d, v.z + d); 124 | } 125 | 126 | public static DVector3 operator+ (double d, DVector3 v) 127 | { 128 | return new DVector3 (v.x + d, v.y + d, v.z + d); 129 | } 130 | 131 | // - 132 | public static DVector3 operator- (DVector3 a, DVector3 b) 133 | { 134 | return new DVector3 (a.x - b.x, a.y - b.y, a.z - b.z); 135 | } 136 | 137 | public static DVector3 operator- (Vector3 a, DVector3 b) 138 | { 139 | return new DVector3 (a.x - b.x, a.y - b.y, a.z - b.z); 140 | } 141 | 142 | public static DVector3 operator- (DVector3 a, Vector3 b) 143 | { 144 | return new DVector3 (a.x - b.x, a.y - b.y, a.z - b.z); 145 | } 146 | 147 | public static DVector3 operator- (DVector3 v, double d) 148 | { 149 | return new DVector3 (v.x - d, v.y - d, v.z - d); 150 | } 151 | 152 | public static DVector3 operator- (double d, DVector3 v) 153 | { 154 | return new DVector3 (v.x - d, v.y - d, v.z - d); 155 | } 156 | 157 | // * 158 | public static DVector3 operator* (DVector3 a, DVector3 b) 159 | { 160 | return new DVector3 (a.x * b.x, a.y * b.y, a.z * b.z); 161 | } 162 | 163 | public static DVector3 operator* (Vector3 a, DVector3 b) 164 | { 165 | return new DVector3 (a.x * b.x, a.y * b.y, a.z * b.z); 166 | } 167 | 168 | public static DVector3 operator* (DVector3 a, Vector3 b) 169 | { 170 | return new DVector3 (a.x * b.x, a.y * b.y, a.z * b.z); 171 | } 172 | 173 | public static DVector3 operator* (DVector3 v, double d) 174 | { 175 | return new DVector3 (v.x * d, v.y * d, v.z * d); 176 | } 177 | 178 | public static DVector3 operator* (double d, DVector3 v) 179 | { 180 | return new DVector3 (v.x * d, v.y * d, v.z * d); 181 | } 182 | 183 | // / 184 | public static DVector3 operator/ (DVector3 a, DVector3 b) 185 | { 186 | return new DVector3 (a.x / b.x, a.y / b.y, a.z / b.z); 187 | } 188 | 189 | public static DVector3 operator/ (Vector3 a, DVector3 b) 190 | { 191 | return new DVector3 (a.x / b.x, a.y / b.y, a.z / b.z); 192 | } 193 | 194 | public static DVector3 operator/ (DVector3 a, Vector3 b) 195 | { 196 | return new DVector3 (a.x / b.x, a.y / b.y, a.z / b.z); 197 | } 198 | 199 | public static DVector3 operator/ (DVector3 v, double d) 200 | { 201 | return new DVector3 (v.x / d, v.y / d, v.z / d); 202 | } 203 | 204 | public static DVector3 operator/ (double d, DVector3 v) 205 | { 206 | return new DVector3 (v.x / d, v.y / d, v.z / d); 207 | } 208 | 209 | // == 210 | public static bool operator== (DVector3 a, DVector3 b) 211 | { 212 | return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); 213 | } 214 | 215 | public static bool operator== (Vector3 a, DVector3 b) 216 | { 217 | return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); 218 | } 219 | 220 | public static bool operator== (DVector3 a, Vector3 b) 221 | { 222 | return (a.x == b.x) && (a.y == b.y) && (a.z == b.z); 223 | } 224 | 225 | // != 226 | public static bool operator!= (DVector3 a, DVector3 b) 227 | { 228 | return (a.x != b.x) || (a.y != b.y) || (a.z == b.z); 229 | } 230 | 231 | public static bool operator!= (Vector3 a, DVector3 b) 232 | { 233 | return (a.x != b.x) || (a.y != b.y) || (a.z == b.z); 234 | } 235 | 236 | public static bool operator!= (DVector3 a, Vector3 b) 237 | { 238 | return (a.x != b.x) || (a.y != b.y) || (a.z == b.z); 239 | } 240 | 241 | #endregion 242 | } 243 | } -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/IMU/DVector3.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: fca9fd74e586a4d3594564b69040e856 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/IMU/IMUObject.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | 3 | [AddComponentMenu("IMU/IMUObject")] 4 | public class IMUObject : MonoBehaviour { 5 | #region privates 6 | Transform _transform; 7 | bool inited = false; 8 | 9 | // rotation 10 | Quaternion initialRotation; 11 | Quaternion gyroInitialRotation; 12 | 13 | #endregion 14 | 15 | #region life cycle 16 | void Start () 17 | { 18 | _transform = this.transform; 19 | 20 | // set gyro 21 | Input.gyro.enabled = true; 22 | Input.gyro.updateInterval = 0.02f; 23 | 24 | initialRotation = _transform.localRotation; 25 | gyroInitialRotation = Input.gyro.attitude; 26 | 27 | } 28 | 29 | void Update () 30 | { 31 | if (!inited) { 32 | inited = gyroInit (); 33 | } 34 | 35 | // rotation 36 | Quaternion offsetRotation = Quaternion.Inverse (gyroInitialRotation) * Input.gyro.attitude; 37 | offsetRotation = this.getEulerInverse (offsetRotation); 38 | _transform.localRotation = initialRotation * offsetRotation; 39 | } 40 | #endregion 41 | 42 | #region private methods 43 | bool gyroInit () 44 | { 45 | Quaternion q = Input.gyro.attitude; 46 | 47 | if (q.x != 0 48 | || q.y != 0 49 | || q.z != 0 50 | || q.w != 0) { 51 | gyroInitialRotation = q; 52 | return true; 53 | } 54 | return false; 55 | } 56 | 57 | // inverse Coordinate system 58 | Quaternion getEulerInverse (Quaternion q) 59 | { 60 | Vector3 e = q.eulerAngles; 61 | e.x *= -1; 62 | e.y *= -1; 63 | return Quaternion.Euler (e); 64 | } 65 | 66 | #endregion 67 | } -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/IMU/IMUObject.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 0833bbc3c8cc744bbbc42e139a4a922f 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/Kalman.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 4becd311ee63643479b676203d1f178c 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/MicAnalyzer.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using System.Collections.Generic; 3 | 4 | /// 5 | /// Mic analyzer. 6 | /// http://forum.unity3d.com/threads/118215-Blow-detection-(Using-iOS-Microphone)?p=801969#post801969 7 | /// 8 | [RequireComponent(typeof(AudioSource))] 9 | [AddComponentMenu("Amid/Sound/MicAnaliyzer")] 10 | public class MicAnalyzer : MonoBehaviour { 11 | 12 | private const int FREQUENCY = 48000; // Wavelength, I think. 13 | private const int SAMPLECOUNT = 1024; // Sample Count. 14 | private const float REFVALUE = 0.1f; // RMS value for 0 dB. 15 | private const float THRESHOLD = 0.02f; // Minimum amplitude to extract pitch (recieve anything) 16 | private const float ALPHA = 0.05f; // The alpha for the low pass filter (I don't really understand this). 17 | 18 | public int recordedLength = 50; // How many previous frames of sound are analyzed. 19 | public int clamp = 160; // Used to clamp dB (I don't really understand this either). 20 | 21 | private float rmsValue; // Volume in RMS 22 | private float dbValue; // Volume in DB 23 | private float pitchValue; // Pitch - Hz (is this frequency?) 24 | 25 | private float lowPassResults; // Low Pass Filter result 26 | private float peakPowerForChannel; // 27 | 28 | private float[] samples; // Samples 29 | private float[] spectrum; // Spectrum 30 | private List dbValues; // Used to average recent volume. 31 | private List pitchValues; // Used to average recent pitch. 32 | 33 | public void Start () 34 | { 35 | samples = new float[SAMPLECOUNT]; 36 | spectrum = new float[SAMPLECOUNT]; 37 | dbValues = new List (); 38 | pitchValues = new List (); 39 | 40 | StartMicListener (); 41 | } 42 | 43 | public void Update () 44 | { 45 | 46 | // If the audio has stopped playing, this will restart the mic play the clip. 47 | if (!audio.isPlaying) { 48 | StartMicListener (); 49 | } 50 | 51 | // Gets volume and pitch values 52 | AnalyzeSound (); 53 | 54 | // Runs a series of algorithms to decide whether a blow is occuring. 55 | DeriveBlow (); 56 | } 57 | 58 | /// Starts the Mic, and plays the audio back in (near) real-time. 59 | private void StartMicListener () 60 | { 61 | audio.clip = Microphone.Start ("Built-in Microphone", true, 999, FREQUENCY); 62 | // HACK - Forces the function to wait until the microphone has started, before moving onto the play function. 63 | while (!(Microphone.GetPosition("Built-in Microphone") > 0)) { 64 | } 65 | audio.Play (); 66 | } 67 | 68 | /// Credits to aldonaletto for the function, http://goo.gl/VGwKt 69 | /// Analyzes the sound, to get volume and pitch values. 70 | private void AnalyzeSound () 71 | { 72 | 73 | // Get all of our samples from the mic. 74 | audio.GetOutputData (samples, 0); 75 | 76 | // Sums squared samples 77 | float sum = 0; 78 | for (int i = 0; i < SAMPLECOUNT; i++) { 79 | sum += Mathf.Pow (samples [i], 2); 80 | } 81 | 82 | // RMS is the square root of the average value of the samples. 83 | rmsValue = Mathf.Sqrt (sum / SAMPLECOUNT); 84 | dbValue = 20 * Mathf.Log10 (rmsValue / REFVALUE); 85 | 86 | // Clamp it to {clamp} min 87 | if (dbValue < -clamp) { 88 | dbValue = -clamp; 89 | } 90 | 91 | // Gets the sound spectrum. 92 | audio.GetSpectrumData (spectrum, 0, FFTWindow.BlackmanHarris); 93 | float maxV = 0; 94 | int maxN = 0; 95 | 96 | // Find the highest sample. 97 | for (int i = 0; i < SAMPLECOUNT; i++) { 98 | if (spectrum [i] > maxV && spectrum [i] > THRESHOLD) { 99 | maxV = spectrum [i]; 100 | maxN = i; // maxN is the index of max 101 | } 102 | } 103 | 104 | // Pass the index to a float variable 105 | float freqN = maxN; 106 | 107 | // Interpolate index using neighbours 108 | if (maxN > 0 && maxN < SAMPLECOUNT - 1) { 109 | float dL = spectrum [maxN - 1] / spectrum [maxN]; 110 | float dR = spectrum [maxN + 1] / spectrum [maxN]; 111 | freqN += 0.5f * (dR * dR - dL * dL); 112 | } 113 | 114 | // Convert index to frequency 115 | pitchValue = freqN * 24000 / SAMPLECOUNT; 116 | } 117 | 118 | private void DeriveBlow () 119 | { 120 | 121 | UpdateRecords (dbValue, dbValues); 122 | UpdateRecords (pitchValue, pitchValues); 123 | 124 | // Find the average pitch in our records (used to decipher against whistles, clicks, etc). 125 | float sumPitch = 0; 126 | foreach (float num in pitchValues) { 127 | sumPitch += num; 128 | } 129 | sumPitch /= pitchValues.Count; 130 | 131 | // Run our low pass filter. 132 | lowPassResults = LowPassFilter (dbValue); 133 | 134 | } 135 | 136 | // Updates a record, by removing the oldest entry and adding the newest value (val). 137 | private void UpdateRecords (float val, List record) 138 | { 139 | if (record.Count > recordedLength) { 140 | record.RemoveAt (0); 141 | } 142 | record.Add (val); 143 | } 144 | 145 | /// Gives a result (I don't really understand this yet) based on the peak volume of the record 146 | /// and the previous low pass results. 147 | private float LowPassFilter (float peakVolume) 148 | { 149 | return ALPHA * peakVolume + (1.0f - ALPHA) * lowPassResults; 150 | } 151 | 152 | 153 | #region Getter 154 | public float[] Samples { 155 | get { 156 | return samples; 157 | } 158 | } 159 | public float[] Spectrum { 160 | get { 161 | return spectrum; 162 | } 163 | } 164 | 165 | public float RMS { 166 | get { 167 | return rmsValue; 168 | } 169 | } 170 | 171 | public float DB { 172 | get { 173 | return dbValue; 174 | } 175 | } 176 | 177 | public float Pitch { 178 | get { 179 | return pitchValue; 180 | } 181 | } 182 | #endregion 183 | 184 | } 185 | -------------------------------------------------------------------------------- /ImuSample/Assets/Scripts/MicAnalyzer.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f621968a70d754711b4b7081b26aaf38 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c9601ef79896d4d2abb1b8f9f88005bb 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 607426a9820414bfd8081fb22854dbd4 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Sunny1 Skybox.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Sunny1 Skybox.mat -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Sunny1 Skybox.mat.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: a2126534b3a11b34e977105bd9e3cc03 3 | NativeFormatImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 9ef50104b12ed4ca9bed40f105986cd7 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 1bf34b610fe6f466faff3dcd833c06f6 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_back.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_back.tif -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_back.tif.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 32ef381921b3f7f479831b042c113762 3 | TextureImporter: 4 | serializedVersion: 2 5 | mipmaps: 6 | mipMapMode: 0 7 | enableMipMap: 1 8 | linearTexture: 0 9 | correctGamma: 0 10 | fadeOut: 0 11 | borderMipMap: 0 12 | mipMapFadeDistanceStart: 1 13 | mipMapFadeDistanceEnd: 3 14 | bumpmap: 15 | convertToNormalMap: 0 16 | externalNormalMap: 0 17 | heightScale: .25 18 | normalMapFilter: 0 19 | isReadable: 0 20 | grayScaleToAlpha: 0 21 | generateCubemap: 0 22 | seamlessCubemap: 0 23 | textureFormat: -3 24 | maxTextureSize: 512 25 | textureSettings: 26 | filterMode: 1 27 | aniso: 1 28 | mipBias: 0 29 | wrapMode: 1 30 | nPOTScale: 1 31 | lightmap: 0 32 | compressionQuality: 50 33 | textureType: 0 34 | buildTargetSettings: [] 35 | userData: 36 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_down.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_down.tif -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_down.tif.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 352e5f82d7105ad408904c5f552b5fe7 3 | TextureImporter: 4 | serializedVersion: 2 5 | mipmaps: 6 | mipMapMode: 0 7 | enableMipMap: 1 8 | linearTexture: 0 9 | correctGamma: 0 10 | fadeOut: 0 11 | borderMipMap: 0 12 | mipMapFadeDistanceStart: 1 13 | mipMapFadeDistanceEnd: 3 14 | bumpmap: 15 | convertToNormalMap: 0 16 | externalNormalMap: 0 17 | heightScale: .25 18 | normalMapFilter: 0 19 | isReadable: 0 20 | grayScaleToAlpha: 0 21 | generateCubemap: 0 22 | seamlessCubemap: 0 23 | textureFormat: -3 24 | maxTextureSize: 512 25 | textureSettings: 26 | filterMode: 1 27 | aniso: 1 28 | mipBias: 0 29 | wrapMode: 1 30 | nPOTScale: 1 31 | lightmap: 0 32 | compressionQuality: 50 33 | textureType: 0 34 | buildTargetSettings: [] 35 | userData: 36 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_front.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_front.tif -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_front.tif.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 2372dd1814cb4ed47a91f8174877623e 3 | TextureImporter: 4 | serializedVersion: 2 5 | mipmaps: 6 | mipMapMode: 0 7 | enableMipMap: 1 8 | linearTexture: 0 9 | correctGamma: 0 10 | fadeOut: 0 11 | borderMipMap: 0 12 | mipMapFadeDistanceStart: 1 13 | mipMapFadeDistanceEnd: 3 14 | bumpmap: 15 | convertToNormalMap: 0 16 | externalNormalMap: 0 17 | heightScale: .25 18 | normalMapFilter: 0 19 | isReadable: 0 20 | grayScaleToAlpha: 0 21 | generateCubemap: 0 22 | seamlessCubemap: 0 23 | textureFormat: -3 24 | maxTextureSize: 512 25 | textureSettings: 26 | filterMode: 1 27 | aniso: 1 28 | mipBias: 0 29 | wrapMode: 1 30 | nPOTScale: 1 31 | lightmap: 0 32 | compressionQuality: 50 33 | textureType: 0 34 | buildTargetSettings: [] 35 | userData: 36 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_left.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_left.tif -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_left.tif.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 66ebe4da1ef67e64e8aeccb820e409d6 3 | TextureImporter: 4 | serializedVersion: 2 5 | mipmaps: 6 | mipMapMode: 0 7 | enableMipMap: 1 8 | linearTexture: 0 9 | correctGamma: 0 10 | fadeOut: 0 11 | borderMipMap: 0 12 | mipMapFadeDistanceStart: 1 13 | mipMapFadeDistanceEnd: 3 14 | bumpmap: 15 | convertToNormalMap: 0 16 | externalNormalMap: 0 17 | heightScale: .25 18 | normalMapFilter: 0 19 | isReadable: 0 20 | grayScaleToAlpha: 0 21 | generateCubemap: 0 22 | seamlessCubemap: 0 23 | textureFormat: -3 24 | maxTextureSize: 512 25 | textureSettings: 26 | filterMode: 1 27 | aniso: 1 28 | mipBias: 0 29 | wrapMode: 1 30 | nPOTScale: 1 31 | lightmap: 0 32 | compressionQuality: 50 33 | textureType: 0 34 | buildTargetSettings: [] 35 | userData: 36 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_right.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_right.tif -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_right.tif.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 195c271a9118d224c8562ae67cf47a66 3 | TextureImporter: 4 | serializedVersion: 2 5 | mipmaps: 6 | mipMapMode: 0 7 | enableMipMap: 1 8 | linearTexture: 0 9 | correctGamma: 0 10 | fadeOut: 0 11 | borderMipMap: 0 12 | mipMapFadeDistanceStart: 1 13 | mipMapFadeDistanceEnd: 3 14 | bumpmap: 15 | convertToNormalMap: 0 16 | externalNormalMap: 0 17 | heightScale: .25 18 | normalMapFilter: 0 19 | isReadable: 0 20 | grayScaleToAlpha: 0 21 | generateCubemap: 0 22 | seamlessCubemap: 0 23 | textureFormat: -3 24 | maxTextureSize: 512 25 | textureSettings: 26 | filterMode: 1 27 | aniso: 1 28 | mipBias: 0 29 | wrapMode: 1 30 | nPOTScale: 1 31 | lightmap: 0 32 | compressionQuality: 50 33 | textureType: 0 34 | buildTargetSettings: [] 35 | userData: 36 | -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_up.tif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_up.tif -------------------------------------------------------------------------------- /ImuSample/Assets/Standard Assets/Skyboxes/Textures/Sunny1/Sunny1_up.tif.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8a98ef45bc7df804d95fd945abd2c943 3 | TextureImporter: 4 | serializedVersion: 2 5 | mipmaps: 6 | mipMapMode: 0 7 | enableMipMap: 1 8 | linearTexture: 0 9 | correctGamma: 0 10 | fadeOut: 0 11 | borderMipMap: 0 12 | mipMapFadeDistanceStart: 1 13 | mipMapFadeDistanceEnd: 3 14 | bumpmap: 15 | convertToNormalMap: 0 16 | externalNormalMap: 0 17 | heightScale: .25 18 | normalMapFilter: 0 19 | isReadable: 0 20 | grayScaleToAlpha: 0 21 | generateCubemap: 0 22 | seamlessCubemap: 0 23 | textureFormat: -3 24 | maxTextureSize: 512 25 | textureSettings: 26 | filterMode: 1 27 | aniso: 1 28 | mipBias: 0 29 | wrapMode: 1 30 | nPOTScale: 1 31 | lightmap: 0 32 | compressionQuality: 50 33 | textureType: 0 34 | buildTargetSettings: [] 35 | userData: 36 | -------------------------------------------------------------------------------- /ImuSample/Assets/TestShaders.unity: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/Assets/TestShaders.unity -------------------------------------------------------------------------------- /ImuSample/Assets/TestShaders.unity.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8440c8c6a7be7b64185b6d365174ef0c 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/AudioManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/AudioManager.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/DynamicsManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/DynamicsManager.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/EditorBuildSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/EditorBuildSettings.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/EditorSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/EditorSettings.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/InputManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/InputManager.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/NavMeshLayers.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/NavMeshLayers.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/NetworkManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/NetworkManager.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/ProjectSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/ProjectSettings.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/QualitySettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/QualitySettings.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/TagManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/TagManager.asset -------------------------------------------------------------------------------- /ImuSample/ProjectSettings/TimeManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/ImuSample/ProjectSettings/TimeManager.asset -------------------------------------------------------------------------------- /ImuSample/README.md: -------------------------------------------------------------------------------- 1 | # Imu Sample 2 | 3 | 開発中です。 4 | 回転クォータニオン部分はできました。 5 | 6 | 回転を同期させたいオブジェクトに、IMUObject.csをAddComponentするだけです。 7 | 8 | 現状、起動時のデバイスの向きが、初期値になるので、 9 | 起動時にデバイスを縦に持ってて下さい。 10 | -------------------------------------------------------------------------------- /KalmanSample/.gitignore: -------------------------------------------------------------------------------- 1 | Library 2 | Temp 3 | *.csproj 4 | *.sln 5 | *.pidb 6 | *.userprefs -------------------------------------------------------------------------------- /KalmanSample/Assets/Material.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 4ea1897a15495468e8a1aac5fd2becf3 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/MouseColorGreen.mat: -------------------------------------------------------------------------------- 1 | %YAML 1.1 2 | %TAG !u! tag:unity3d.com,2011: 3 | --- !u!21 &2100000 4 | Material: 5 | serializedVersion: 3 6 | m_ObjectHideFlags: 0 7 | m_PrefabParentObject: {fileID: 0} 8 | m_PrefabInternal: {fileID: 0} 9 | m_Name: MouseColorGreen 10 | m_Shader: {fileID: 30, guid: 0000000000000000e000000000000000, type: 0} 11 | m_SavedProperties: 12 | serializedVersion: 2 13 | m_TexEnvs: 14 | data: 15 | first: 16 | name: _MainTex 17 | second: 18 | m_Texture: {fileID: 0} 19 | m_Scale: {x: 1, y: 1} 20 | m_Offset: {x: 0, y: 0} 21 | m_Floats: {} 22 | m_Colors: 23 | data: 24 | first: 25 | name: _Color 26 | second: {r: .166908011, g: .604477644, b: .384162903, a: .505882382} 27 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/MouseColorGreen.mat.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 80e63ac7dbfda43e7a248e077185dfff 3 | NativeFormatImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/MouseColorRed.mat: -------------------------------------------------------------------------------- 1 | %YAML 1.1 2 | %TAG !u! tag:unity3d.com,2011: 3 | --- !u!21 &2100000 4 | Material: 5 | serializedVersion: 3 6 | m_ObjectHideFlags: 0 7 | m_PrefabParentObject: {fileID: 0} 8 | m_PrefabInternal: {fileID: 0} 9 | m_Name: MouseColorRed 10 | m_Shader: {fileID: 30, guid: 0000000000000000e000000000000000, type: 0} 11 | m_SavedProperties: 12 | serializedVersion: 2 13 | m_TexEnvs: 14 | data: 15 | first: 16 | name: _MainTex 17 | second: 18 | m_Texture: {fileID: 0} 19 | m_Scale: {x: 1, y: 1} 20 | m_Offset: {x: 0, y: 0} 21 | m_Floats: {} 22 | m_Colors: 23 | data: 24 | first: 25 | name: _Color 26 | second: {r: .604477644, g: .166908011, b: .451481253, a: .498039216} 27 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/MouseColorRed.mat.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f3398b30a088c4bee950f599c6249b3a 3 | NativeFormatImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/TrailGreen.mat: -------------------------------------------------------------------------------- 1 | %YAML 1.1 2 | %TAG !u! tag:unity3d.com,2011: 3 | --- !u!21 &2100000 4 | Material: 5 | serializedVersion: 3 6 | m_ObjectHideFlags: 0 7 | m_PrefabParentObject: {fileID: 0} 8 | m_PrefabInternal: {fileID: 0} 9 | m_Name: TrailGreen 10 | m_Shader: {fileID: 200, guid: 0000000000000000e000000000000000, type: 0} 11 | m_SavedProperties: 12 | serializedVersion: 2 13 | m_TexEnvs: 14 | data: 15 | first: 16 | name: _MainTex 17 | second: 18 | m_Texture: {fileID: 0} 19 | m_Scale: {x: 1, y: 1} 20 | m_Offset: {x: 0, y: 0} 21 | m_Floats: 22 | data: 23 | first: 24 | name: _InvFade 25 | second: 1 26 | m_Colors: 27 | data: 28 | first: 29 | name: _Color 30 | second: {r: 1, g: 1, b: 1, a: 1} 31 | data: 32 | first: 33 | name: _TintColor 34 | second: {r: .0658832937, g: .679104447, b: .344620198, a: .501960814} 35 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/TrailGreen.mat.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 371398fc62d5c43808b4a4028d9c6112 3 | NativeFormatImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/TrailRed.mat: -------------------------------------------------------------------------------- 1 | %YAML 1.1 2 | %TAG !u! tag:unity3d.com,2011: 3 | --- !u!21 &2100000 4 | Material: 5 | serializedVersion: 3 6 | m_ObjectHideFlags: 0 7 | m_PrefabParentObject: {fileID: 0} 8 | m_PrefabInternal: {fileID: 0} 9 | m_Name: TrailRed 10 | m_Shader: {fileID: 200, guid: 0000000000000000e000000000000000, type: 0} 11 | m_SavedProperties: 12 | serializedVersion: 2 13 | m_TexEnvs: 14 | data: 15 | first: 16 | name: _MainTex 17 | second: 18 | m_Texture: {fileID: 0} 19 | m_Scale: {x: 1, y: 1} 20 | m_Offset: {x: 0, y: 0} 21 | m_Floats: 22 | data: 23 | first: 24 | name: _InvFade 25 | second: 1 26 | m_Colors: 27 | data: 28 | first: 29 | name: _Color 30 | second: {r: 1, g: 1, b: 1, a: 1} 31 | data: 32 | first: 33 | name: _TintColor 34 | second: {r: .679104447, g: .0658832565, b: .0658832565, a: .501960814} 35 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Material/TrailRed.mat.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 64c943997ccef444e9b4ea7da414fcfa 3 | NativeFormatImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: abd24ac5d04fd4fdc85d4b1400f761c7 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 69fe5870eb1e14969aef28eaae6c7cd1 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/IKalmanWrapper.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using System.Collections; 3 | 4 | 5 | namespace Kalman { 6 | public interface IKalmanWrapper : System.IDisposable 7 | { 8 | Vector3 Update (Vector3 current); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/IKalmanWrapper.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 53422befabcbe43f6996640164caa05d 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Math.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: bf2857c3cc9014a2f90ad56871373d11 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Math/KalmanFilter.cs: -------------------------------------------------------------------------------- 1 | namespace Kalman 2 | { 3 | public sealed class KalmanFilter 4 | { 5 | //System matrices 6 | public Matrix X0 { get; private set; } // predicted state 7 | public Matrix P0 { get; private set; } // predicted covariance 8 | 9 | public Matrix F { get; private set; } // factor of real value to previous real value 10 | public Matrix B { get; private set; } // the control-input model which is applied to the control vector uk; 11 | public Matrix U { get; private set; } // the control-input model which is applied to the control vector uk; 12 | public Matrix Q { get; private set; } // measurement noise 13 | public Matrix H { get; private set; } // factor of measured value to real value 14 | public Matrix R { get; private set; } // environment noise 15 | 16 | public Matrix State { get; private set; } 17 | public Matrix Covariance { get; private set; } 18 | 19 | public KalmanFilter(Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r) 20 | { 21 | F = f; 22 | B = b; 23 | U = u; 24 | Q = q; 25 | H = h; 26 | R = r; 27 | } 28 | 29 | public void SetState(Matrix state, Matrix covariance) 30 | { 31 | // Set initial state 32 | State = state; 33 | Covariance = covariance; 34 | } 35 | 36 | public void Correct (Matrix z) 37 | { 38 | // Predict 39 | //X0 = F * State +(B * U); 40 | X0 = F * State; 41 | P0 = F * Covariance * F.Transpose () + Q; 42 | 43 | // Correct 44 | //var k = P0 * H.Transpose() * (H * P0 * H.Transpose() + R).Inverse(); // kalman gain 45 | var k = P0 * H.Transpose () * (H * P0 * H.Transpose () + R).Invert (); // kalman gain 46 | State = X0 + (k * (z - (H * X0))); 47 | //Covariance = (Matrix.Identity (P0.RowCount) - k * H) * P0; 48 | Covariance = (Matrix.IdentityMatrix (P0.rows) - k * H) * P0; 49 | } 50 | } 51 | } -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Math/KalmanFilter.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 9cd0cd65c589c49c4b0abab5ce3d1719 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Math/KalmanFilterSimple1D.cs: -------------------------------------------------------------------------------- 1 | namespace Kalman 2 | { 3 | class KalmanFilterSimple1D 4 | { 5 | public double X0 { get; private set; } // predicted state 6 | public double P0 { get; private set; } // predicted covariance 7 | 8 | public double F { get; private set; } // factor of real value to previous real value 9 | public double Q { get; private set; } // measurement noise 10 | public double H { get; private set; } // factor of measured value to real value 11 | public double R { get; private set; } // environment noise 12 | 13 | public double State { get; private set; } 14 | public double Covariance { get; private set; } 15 | 16 | public KalmanFilterSimple1D(double q, double r, double f = 1, double h = 1) 17 | { 18 | Q = q; 19 | R = r; 20 | F = f; 21 | H = h; 22 | } 23 | 24 | public void SetState(double state, double covariance) 25 | { 26 | State = state; 27 | Covariance = covariance; 28 | } 29 | 30 | public void Correct(double data) 31 | { 32 | //time update - prediction 33 | X0 = F * State; 34 | P0 = F * Covariance * F + Q; 35 | 36 | //measurement update - correction 37 | var K = H * P0 / (H * P0 * H + R); 38 | State = X0 + K * (data - H * X0); 39 | Covariance = (1 - K * H) * P0; 40 | } 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Math/KalmanFilterSimple1D.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: ca6eef9362ffd485496a5f649d16195e 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Math/Matrix.cs: -------------------------------------------------------------------------------- 1 | /* 2 | Matrix class in C# 3 | Written by Ivan Kuckir (ivan.kuckir@gmail.com, www.ivank.net/en) 4 | Faculty of Mathematics and Physics 5 | Charles University in Prague 6 | (C) 2010 7 | - updated on 14.6.2012 - parsing improved. Thanks to Andy! 8 | - updated on 3.10.2012 - there was a terrible bug in LU, SoLE and Inversion. Thanks to Danilo Neves Cruz for reporting that! 9 | */ 10 | 11 | using System; 12 | using System.Text.RegularExpressions; 13 | 14 | namespace Kalman { 15 | public class Matrix 16 | { 17 | 18 | public int rows; 19 | public int cols; 20 | public double[,] mat; 21 | 22 | public Matrix L; 23 | public Matrix U; 24 | private int[] pi; 25 | private double detOfP = 1; 26 | 27 | public Matrix (int iRows, int iCols) // Matrix Class constructor 28 | { 29 | rows = iRows; 30 | cols = iCols; 31 | mat = new double[rows, cols]; 32 | } 33 | 34 | public Matrix (double[,] matrix) 35 | { 36 | rows = matrix.GetLength (0); 37 | cols = matrix.GetLength (1); 38 | mat = matrix; 39 | } 40 | 41 | public Boolean IsSquare() 42 | { 43 | return (rows == cols); 44 | } 45 | 46 | public double this[int iRow, int iCol] // Access this matrix as a 2D array 47 | { 48 | get { return mat[iRow, iCol]; } 49 | set { mat[iRow, iCol] = value; } 50 | } 51 | 52 | public Matrix GetCol(int k) 53 | { 54 | Matrix m = new Matrix(rows, 1); 55 | for (int i = 0; i < rows; i++) m[i, 0] = mat[i, k]; 56 | return m; 57 | } 58 | 59 | public void SetCol(Matrix v, int k) 60 | { 61 | for (int i = 0; i < rows; i++) mat[i, k] = v[i, 0]; 62 | } 63 | 64 | public void MakeLU() // Function for LU decomposition 65 | { 66 | if (!IsSquare()) throw new MException("The matrix is not square!"); 67 | L = IdentityMatrix(rows, cols); 68 | U = Duplicate(); 69 | 70 | pi = new int[rows]; 71 | for (int i = 0; i < rows; i++) pi[i] = i; 72 | 73 | double p = 0; 74 | double pom2; 75 | int k0 = 0; 76 | int pom1 = 0; 77 | 78 | for (int k = 0; k < cols - 1; k++) 79 | { 80 | p = 0; 81 | for (int i = k; i < rows; i++) // find the row with the biggest pivot 82 | { 83 | if (Math.Abs(U[i, k]) > p) 84 | { 85 | p = Math.Abs(U[i, k]); 86 | k0 = i; 87 | } 88 | } 89 | if (p == 0) // samé nuly ve sloupci 90 | throw new MException("The matrix is singular!"); 91 | 92 | pom1 = pi[k]; pi[k] = pi[k0]; pi[k0] = pom1; // switch two rows in permutation matrix 93 | 94 | for (int i = 0; i < k; i++) 95 | { 96 | pom2 = L[k, i]; L[k, i] = L[k0, i]; L[k0, i] = pom2; 97 | } 98 | 99 | if (k != k0) detOfP *= -1; 100 | 101 | for (int i = 0; i < cols; i++) // Switch rows in U 102 | { 103 | pom2 = U[k, i]; U[k, i] = U[k0, i]; U[k0, i] = pom2; 104 | } 105 | 106 | for (int i = k + 1; i < rows; i++) 107 | { 108 | L[i, k] = U[i, k] / U[k, k]; 109 | for (int j = k; j < cols; j++) 110 | U[i, j] = U[i, j] - L[i, k] * U[k, j]; 111 | } 112 | } 113 | } 114 | 115 | 116 | public Matrix SolveWith(Matrix v) // Function solves Ax = v in confirmity with solution vector "v" 117 | { 118 | if (rows != cols) throw new MException("The matrix is not square!"); 119 | if (rows != v.rows) throw new MException("Wrong number of results in solution vector!"); 120 | if (L == null) MakeLU(); 121 | 122 | Matrix b = new Matrix(rows, 1); 123 | for (int i = 0; i < rows; i++) b[i, 0] = v[pi[i], 0]; // switch two items in "v" due to permutation matrix 124 | 125 | Matrix z = SubsForth(L, b); 126 | Matrix x = SubsBack(U, z); 127 | 128 | return x; 129 | } 130 | 131 | public Matrix Invert() // Function returns the inverted matrix 132 | { 133 | if (L == null) MakeLU(); 134 | 135 | Matrix inv = new Matrix(rows, cols); 136 | 137 | for (int i = 0; i < rows; i++) 138 | { 139 | Matrix Ei = Matrix.ZeroMatrix(rows, 1); 140 | Ei[i, 0] = 1; 141 | Matrix col = SolveWith(Ei); 142 | inv.SetCol(col, i); 143 | } 144 | return inv; 145 | } 146 | 147 | 148 | public double Det() // Function for determinant 149 | { 150 | if (L == null) MakeLU(); 151 | double det = detOfP; 152 | for (int i = 0; i < rows; i++) det *= U[i, i]; 153 | return det; 154 | } 155 | 156 | public Matrix GetP() // Function returns permutation matrix "P" due to permutation vector "pi" 157 | { 158 | if (L == null) MakeLU(); 159 | 160 | Matrix matrix = ZeroMatrix(rows, cols); 161 | for (int i = 0; i < rows; i++) matrix[pi[i], i] = 1; 162 | return matrix; 163 | } 164 | 165 | public Matrix Duplicate() // Function returns the copy of this matrix 166 | { 167 | Matrix matrix = new Matrix(rows, cols); 168 | for (int i = 0; i < rows; i++) 169 | for (int j = 0; j < cols; j++) 170 | matrix[i, j] = mat[i, j]; 171 | return matrix; 172 | } 173 | 174 | public static Matrix SubsForth(Matrix A, Matrix b) // Function solves Ax = b for A as a lower triangular matrix 175 | { 176 | if (A.L == null) A.MakeLU(); 177 | int n = A.rows; 178 | Matrix x = new Matrix(n, 1); 179 | 180 | for (int i = 0; i < n; i++) 181 | { 182 | x[i, 0] = b[i, 0]; 183 | for (int j = 0; j < i; j++) x[i, 0] -= A[i, j] * x[j, 0]; 184 | x[i, 0] = x[i, 0] / A[i, i]; 185 | } 186 | return x; 187 | } 188 | 189 | public static Matrix SubsBack(Matrix A, Matrix b) // Function solves Ax = b for A as an upper triangular matrix 190 | { 191 | if (A.L == null) A.MakeLU(); 192 | int n = A.rows; 193 | Matrix x = new Matrix(n, 1); 194 | 195 | for (int i = n - 1; i > -1; i--) 196 | { 197 | x[i, 0] = b[i, 0]; 198 | for (int j = n - 1; j > i; j--) x[i, 0] -= A[i, j] * x[j, 0]; 199 | x[i, 0] = x[i, 0] / A[i, i]; 200 | } 201 | return x; 202 | } 203 | 204 | public static Matrix ZeroMatrix(int iRows, int iCols) // Function generates the zero matrix 205 | { 206 | Matrix matrix = new Matrix(iRows, iCols); 207 | for (int i = 0; i < iRows; i++) 208 | for (int j = 0; j < iCols; j++) 209 | matrix[i, j] = 0; 210 | return matrix; 211 | } 212 | 213 | public static Matrix IdentityMatrix (int iRows, int iCols) // Function generates the identity matrix 214 | { 215 | Matrix matrix = ZeroMatrix (iRows, iCols); 216 | for (int i = 0; i < Math.Min(iRows, iCols); i++) 217 | matrix [i, i] = 1; 218 | return matrix; 219 | } 220 | 221 | public static Matrix IdentityMatrix (int size) 222 | { 223 | return IdentityMatrix (size, size); 224 | } 225 | 226 | public static Matrix RandomMatrix(int iRows, int iCols, int dispersion) // Function generates the zero matrix 227 | { 228 | Random random = new Random(); 229 | Matrix matrix = new Matrix(iRows, iCols); 230 | for (int i = 0; i < iRows; i++) 231 | for (int j = 0; j < iCols; j++) 232 | matrix[i, j] = random.Next(-dispersion, dispersion); 233 | return matrix; 234 | } 235 | 236 | public static Matrix Parse(string ps) // Function parses the matrix from string 237 | { 238 | string s = NormalizeMatrixString(ps); 239 | string[] rows = Regex.Split(s, "\r\n"); 240 | string[] nums = rows[0].Split(' '); 241 | Matrix matrix = new Matrix(rows.Length, nums.Length); 242 | try 243 | { 244 | for (int i = 0; i < rows.Length; i++) 245 | { 246 | nums = rows[i].Split(' '); 247 | for (int j = 0; j < nums.Length; j++) matrix[i, j] = double.Parse(nums[j]); 248 | } 249 | } 250 | catch (FormatException exc) { throw new MException("Wrong input format! "+exc.ToString()); } 251 | return matrix; 252 | } 253 | 254 | public override string ToString() // Function returns matrix as a string 255 | { 256 | string s = ""; 257 | for (int i = 0; i < rows; i++) 258 | { 259 | for (int j = 0; j < cols; j++) s += String.Format("{0,5:0.00}", mat[i, j]) + " "; 260 | s += "\r\n"; 261 | } 262 | return s; 263 | } 264 | 265 | public static Matrix Transpose (Matrix m) // Matrix transpose, for any rectangular matrix 266 | { 267 | Matrix t = new Matrix (m.cols, m.rows); 268 | for (int i = 0; i < m.rows; i++) 269 | for (int j = 0; j < m.cols; j++) 270 | t [j, i] = m [i, j]; 271 | return t; 272 | } 273 | 274 | public Matrix Transpose () 275 | { 276 | return Matrix.Transpose (this); 277 | } 278 | 279 | public static Matrix Power(Matrix m, int pow) // Power matrix to exponent 280 | { 281 | if (pow == 0) return IdentityMatrix(m.rows, m.cols); 282 | if (pow == 1) return m.Duplicate(); 283 | if (pow == -1) return m.Invert(); 284 | 285 | Matrix x; 286 | if (pow < 0) { x = m.Invert(); pow *= -1; } 287 | else x = m.Duplicate(); 288 | 289 | Matrix ret = IdentityMatrix(m.rows, m.cols); 290 | while (pow != 0) 291 | { 292 | if ((pow & 1) == 1) ret *= x; 293 | x *= x; 294 | pow >>= 1; 295 | } 296 | return ret; 297 | } 298 | 299 | private static void SafeAplusBintoC(Matrix A, int xa, int ya, Matrix B, int xb, int yb, Matrix C, int size) 300 | { 301 | for (int i = 0; i < size; i++) // rows 302 | for (int j = 0; j < size; j++) // cols 303 | { 304 | C[i, j] = 0; 305 | if (xa + j < A.cols && ya + i < A.rows) C[i, j] += A[ya + i, xa + j]; 306 | if (xb + j < B.cols && yb + i < B.rows) C[i, j] += B[yb + i, xb + j]; 307 | } 308 | } 309 | 310 | private static void SafeAminusBintoC(Matrix A, int xa, int ya, Matrix B, int xb, int yb, Matrix C, int size) 311 | { 312 | for (int i = 0; i < size; i++) // rows 313 | for (int j = 0; j < size; j++) // cols 314 | { 315 | C[i, j] = 0; 316 | if (xa + j < A.cols && ya + i < A.rows) C[i, j] += A[ya + i, xa + j]; 317 | if (xb + j < B.cols && yb + i < B.rows) C[i, j] -= B[yb + i, xb + j]; 318 | } 319 | } 320 | 321 | private static void SafeACopytoC(Matrix A, int xa, int ya, Matrix C, int size) 322 | { 323 | for (int i = 0; i < size; i++) // rows 324 | for (int j = 0; j < size; j++) // cols 325 | { 326 | C[i, j] = 0; 327 | if (xa + j < A.cols && ya + i < A.rows) C[i, j] += A[ya + i, xa + j]; 328 | } 329 | } 330 | 331 | private static void AplusBintoC(Matrix A, int xa, int ya, Matrix B, int xb, int yb, Matrix C, int size) 332 | { 333 | for (int i = 0; i < size; i++) // rows 334 | for (int j = 0; j < size; j++) C[i, j] = A[ya + i, xa + j] + B[yb + i, xb + j]; 335 | } 336 | 337 | private static void AminusBintoC(Matrix A, int xa, int ya, Matrix B, int xb, int yb, Matrix C, int size) 338 | { 339 | for (int i = 0; i < size; i++) // rows 340 | for (int j = 0; j < size; j++) C[i, j] = A[ya + i, xa + j] - B[yb + i, xb + j]; 341 | } 342 | 343 | private static void ACopytoC(Matrix A, int xa, int ya, Matrix C, int size) 344 | { 345 | for (int i = 0; i < size; i++) // rows 346 | for (int j = 0; j < size; j++) C[i, j] = A[ya + i, xa + j]; 347 | } 348 | 349 | private static Matrix StrassenMultiply(Matrix A, Matrix B) // Smart matrix multiplication 350 | { 351 | if (A.cols != B.rows) throw new MException("Wrong dimension of matrix!"); 352 | 353 | Matrix R; 354 | 355 | int msize = Math.Max(Math.Max(A.rows, A.cols), Math.Max(B.rows, B.cols)); 356 | 357 | if (msize < 32) 358 | { 359 | R = ZeroMatrix(A.rows, B.cols); 360 | for (int i = 0; i < R.rows; i++) 361 | for (int j = 0; j < R.cols; j++) 362 | for (int k = 0; k < A.cols; k++) 363 | R[i, j] += A[i, k] * B[k, j]; 364 | return R; 365 | } 366 | 367 | int size = 1; int n = 0; 368 | while (msize > size) { size *= 2; n++; }; 369 | int h = size / 2; 370 | 371 | 372 | Matrix[,] mField = new Matrix[n, 9]; 373 | 374 | /* 375 | * 8x8, 8x8, 8x8, ... 376 | * 4x4, 4x4, 4x4, ... 377 | * 2x2, 2x2, 2x2, ... 378 | * . . . 379 | */ 380 | 381 | int z; 382 | for (int i = 0; i < n - 4; i++) // rows 383 | { 384 | z = (int)Math.Pow(2, n - i - 1); 385 | for (int j = 0; j < 9; j++) mField[i, j] = new Matrix(z, z); 386 | } 387 | 388 | SafeAplusBintoC(A, 0, 0, A, h, h, mField[0, 0], h); 389 | SafeAplusBintoC(B, 0, 0, B, h, h, mField[0, 1], h); 390 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 1], 1, mField); // (A11 + A22) * (B11 + B22); 391 | 392 | SafeAplusBintoC(A, 0, h, A, h, h, mField[0, 0], h); 393 | SafeACopytoC(B, 0, 0, mField[0, 1], h); 394 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 2], 1, mField); // (A21 + A22) * B11; 395 | 396 | SafeACopytoC(A, 0, 0, mField[0, 0], h); 397 | SafeAminusBintoC(B, h, 0, B, h, h, mField[0, 1], h); 398 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 3], 1, mField); //A11 * (B12 - B22); 399 | 400 | SafeACopytoC(A, h, h, mField[0, 0], h); 401 | SafeAminusBintoC(B, 0, h, B, 0, 0, mField[0, 1], h); 402 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 4], 1, mField); //A22 * (B21 - B11); 403 | 404 | SafeAplusBintoC(A, 0, 0, A, h, 0, mField[0, 0], h); 405 | SafeACopytoC(B, h, h, mField[0, 1], h); 406 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 5], 1, mField); //(A11 + A12) * B22; 407 | 408 | SafeAminusBintoC(A, 0, h, A, 0, 0, mField[0, 0], h); 409 | SafeAplusBintoC(B, 0, 0, B, h, 0, mField[0, 1], h); 410 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 6], 1, mField); //(A21 - A11) * (B11 + B12); 411 | 412 | SafeAminusBintoC(A, h, 0, A, h, h, mField[0, 0], h); 413 | SafeAplusBintoC(B, 0, h, B, h, h, mField[0, 1], h); 414 | StrassenMultiplyRun(mField[0, 0], mField[0, 1], mField[0, 1 + 7], 1, mField); // (A12 - A22) * (B21 + B22); 415 | 416 | R = new Matrix(A.rows, B.cols); // result 417 | 418 | /// C11 419 | for (int i = 0; i < Math.Min(h, R.rows); i++) // rows 420 | for (int j = 0; j < Math.Min(h, R.cols); j++) // cols 421 | R[i, j] = mField[0, 1 + 1][i, j] + mField[0, 1 + 4][i, j] - mField[0, 1 + 5][i, j] + mField[0, 1 + 7][i, j]; 422 | 423 | /// C12 424 | for (int i = 0; i < Math.Min(h, R.rows); i++) // rows 425 | for (int j = h; j < Math.Min(2 * h, R.cols); j++) // cols 426 | R[i, j] = mField[0, 1 + 3][i, j - h] + mField[0, 1 + 5][i, j - h]; 427 | 428 | /// C21 429 | for (int i = h; i < Math.Min(2 * h, R.rows); i++) // rows 430 | for (int j = 0; j < Math.Min(h, R.cols); j++) // cols 431 | R[i, j] = mField[0, 1 + 2][i - h, j] + mField[0, 1 + 4][i - h, j]; 432 | 433 | /// C22 434 | for (int i = h; i < Math.Min(2 * h, R.rows); i++) // rows 435 | for (int j = h; j < Math.Min(2 * h, R.cols); j++) // cols 436 | R[i, j] = mField[0, 1 + 1][i - h, j - h] - mField[0, 1 + 2][i - h, j - h] + mField[0, 1 + 3][i - h, j - h] + mField[0, 1 + 6][i - h, j - h]; 437 | 438 | return R; 439 | } 440 | 441 | // function for square matrix 2^N x 2^N 442 | 443 | private static void StrassenMultiplyRun(Matrix A, Matrix B, Matrix C, int l, Matrix[,] f) // A * B into C, level of recursion, matrix field 444 | { 445 | int size = A.rows; 446 | int h = size / 2; 447 | 448 | if (size < 32) 449 | { 450 | for (int i = 0; i < C.rows; i++) 451 | for (int j = 0; j < C.cols; j++) 452 | { 453 | C[i, j] = 0; 454 | for (int k = 0; k < A.cols; k++) C[i, j] += A[i, k] * B[k, j]; 455 | } 456 | return; 457 | } 458 | 459 | AplusBintoC(A, 0, 0, A, h, h, f[l, 0], h); 460 | AplusBintoC(B, 0, 0, B, h, h, f[l, 1], h); 461 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 1], l + 1, f); // (A11 + A22) * (B11 + B22); 462 | 463 | AplusBintoC(A, 0, h, A, h, h, f[l, 0], h); 464 | ACopytoC(B, 0, 0, f[l, 1], h); 465 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 2], l + 1, f); // (A21 + A22) * B11; 466 | 467 | ACopytoC(A, 0, 0, f[l, 0], h); 468 | AminusBintoC(B, h, 0, B, h, h, f[l, 1], h); 469 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 3], l + 1, f); //A11 * (B12 - B22); 470 | 471 | ACopytoC(A, h, h, f[l, 0], h); 472 | AminusBintoC(B, 0, h, B, 0, 0, f[l, 1], h); 473 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 4], l + 1, f); //A22 * (B21 - B11); 474 | 475 | AplusBintoC(A, 0, 0, A, h, 0, f[l, 0], h); 476 | ACopytoC(B, h, h, f[l, 1], h); 477 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 5], l + 1, f); //(A11 + A12) * B22; 478 | 479 | AminusBintoC(A, 0, h, A, 0, 0, f[l, 0], h); 480 | AplusBintoC(B, 0, 0, B, h, 0, f[l, 1], h); 481 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 6], l + 1, f); //(A21 - A11) * (B11 + B12); 482 | 483 | AminusBintoC(A, h, 0, A, h, h, f[l, 0], h); 484 | AplusBintoC(B, 0, h, B, h, h, f[l, 1], h); 485 | StrassenMultiplyRun(f[l, 0], f[l, 1], f[l, 1 + 7], l + 1, f); // (A12 - A22) * (B21 + B22); 486 | 487 | /// C11 488 | for (int i = 0; i < h; i++) // rows 489 | for (int j = 0; j < h; j++) // cols 490 | C[i, j] = f[l, 1 + 1][i, j] + f[l, 1 + 4][i, j] - f[l, 1 + 5][i, j] + f[l, 1 + 7][i, j]; 491 | 492 | /// C12 493 | for (int i = 0; i < h; i++) // rows 494 | for (int j = h; j < size; j++) // cols 495 | C[i, j] = f[l, 1 + 3][i, j - h] + f[l, 1 + 5][i, j - h]; 496 | 497 | /// C21 498 | for (int i = h; i < size; i++) // rows 499 | for (int j = 0; j < h; j++) // cols 500 | C[i, j] = f[l, 1 + 2][i - h, j] + f[l, 1 + 4][i - h, j]; 501 | 502 | /// C22 503 | for (int i = h; i < size; i++) // rows 504 | for (int j = h; j < size; j++) // cols 505 | C[i, j] = f[l, 1 + 1][i - h, j - h] - f[l, 1 + 2][i - h, j - h] + f[l, 1 + 3][i - h, j - h] + f[l, 1 + 6][i - h, j - h]; 506 | } 507 | 508 | public static Matrix StupidMultiply(Matrix m1, Matrix m2) // Stupid matrix multiplication 509 | { 510 | if (m1.cols != m2.rows) throw new MException("Wrong dimensions of matrix!"); 511 | 512 | Matrix result = ZeroMatrix(m1.rows, m2.cols); 513 | for (int i = 0; i < result.rows; i++) 514 | for (int j = 0; j < result.cols; j++) 515 | for (int k = 0; k < m1.cols; k++) 516 | result[i, j] += m1[i, k] * m2[k, j]; 517 | return result; 518 | } 519 | private static Matrix Multiply(double n, Matrix m) // Multiplication by constant n 520 | { 521 | Matrix r = new Matrix(m.rows, m.cols); 522 | for (int i = 0; i < m.rows; i++) 523 | for (int j = 0; j < m.cols; j++) 524 | r[i, j] = m[i, j] * n; 525 | return r; 526 | } 527 | private static Matrix Add(Matrix m1, Matrix m2) // Sčítání matic 528 | { 529 | if (m1.rows != m2.rows || m1.cols != m2.cols) throw new MException("Matrices must have the same dimensions!"); 530 | Matrix r = new Matrix(m1.rows, m1.cols); 531 | for (int i = 0; i < r.rows; i++) 532 | for (int j = 0; j < r.cols; j++) 533 | r[i, j] = m1[i, j] + m2[i, j]; 534 | return r; 535 | } 536 | 537 | public static string NormalizeMatrixString(string matStr) // From Andy - thank you! :) 538 | { 539 | // Remove any multiple spaces 540 | while (matStr.IndexOf(" ") != -1) 541 | matStr = matStr.Replace(" ", " "); 542 | 543 | // Remove any spaces before or after newlines 544 | matStr = matStr.Replace(" \r\n", "\r\n"); 545 | matStr = matStr.Replace("\r\n ", "\r\n"); 546 | 547 | // If the data ends in a newline, remove the trailing newline. 548 | // Make it easier by first replacing \r\n’s with |’s then 549 | // restore the |’s with \r\n’s 550 | matStr = matStr.Replace("\r\n", "|"); 551 | while (matStr.LastIndexOf("|") == (matStr.Length - 1)) 552 | matStr = matStr.Substring(0, matStr.Length - 1); 553 | 554 | matStr = matStr.Replace("|", "\r\n"); 555 | return matStr; 556 | } 557 | 558 | public static Matrix CreateVector (params double[] values) 559 | { 560 | var matrix = new double[values.Length, 1]; 561 | for (int i=0; i 6 | /// Matrix kalman wrapper. 7 | /// 8 | public class MatrixKalmanWrapper : IKalmanWrapper 9 | { 10 | private KalmanFilter kX; 11 | private KalmanFilter kY; 12 | private KalmanFilter kZ; 13 | 14 | public MatrixKalmanWrapper () 15 | { 16 | /* 17 | X0 : predicted state 18 | P0 : predicted covariance 19 | 20 | F : factor of real value to previous real value 21 | B : the control-input model which is applied to the control vector uk; 22 | U : the control-input model which is applied to the control vector uk; 23 | Q : measurement noise 24 | H : factor of measured value to real value 25 | R : environment noise 26 | */ 27 | var f = new Matrix (new[,] {{1.0, 1}, {0, 1.0}}); 28 | var b = new Matrix (new[,] {{0.0}, {0}}); 29 | var u = new Matrix (new[,] {{0.0}, {0}}); 30 | var r = Matrix.CreateVector (10); 31 | var q = new Matrix (new[,] {{0.01, 0.4}, {0.1, 0.02}}); 32 | var h = new Matrix (new[,] {{1.0 , 0}}); 33 | 34 | kX = makeKalmanFilter (f, b, u, q, h, r); 35 | kY = makeKalmanFilter (f, b, u, q, h, r); 36 | kZ = makeKalmanFilter (f, b, u, q, h, r); 37 | } 38 | 39 | public Vector3 Update (Vector3 current) 40 | { 41 | kX.Correct (new Matrix (new double[,] {{current.x}})); 42 | kY.Correct (new Matrix (new double[,] {{current.y}})); 43 | kZ.Correct (new Matrix (new double[,] {{current.z}})); 44 | 45 | // rashod 46 | // kX.State [1,0]; 47 | // kY.State [1,0]; 48 | // kZ.State [1,0]; 49 | 50 | Vector3 filtered = new Vector3 ( 51 | (float)kX.State [0, 0], 52 | (float)kY.State [0, 0], 53 | (float)kZ.State [0, 0] 54 | ); 55 | return filtered; 56 | } 57 | 58 | public void Dispose () 59 | { 60 | 61 | } 62 | 63 | #region Privates 64 | KalmanFilter makeKalmanFilter (Matrix f, Matrix b, Matrix u, Matrix q, Matrix h, Matrix r) 65 | { 66 | var filter = new KalmanFilter ( 67 | f.Duplicate (), 68 | b.Duplicate (), 69 | u.Duplicate (), 70 | q.Duplicate (), 71 | h.Duplicate (), 72 | r.Duplicate () 73 | ); 74 | // set initial value 75 | filter.SetState ( 76 | Matrix.CreateVector (500, 0), 77 | new Matrix (new [,] {{10.0, 0}, {0, 5.0}}) 78 | ); 79 | return filter; 80 | } 81 | #endregion 82 | 83 | 84 | 85 | } 86 | 87 | } 88 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/MatrixKalmanWrapper.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 3f268de2e7dba49de9f413c2e8131fcc 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/SimpleKalmanWrapper.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | 3 | namespace Kalman { 4 | 5 | /// 6 | /// Simple kalman wrapper. 7 | /// 8 | public class SimpleKalmanWrapper : IKalmanWrapper { 9 | 10 | private KalmanFilterSimple1D kX; 11 | private KalmanFilterSimple1D kY; 12 | private KalmanFilterSimple1D kZ; 13 | 14 | public SimpleKalmanWrapper () 15 | { 16 | /* 17 | X0 : predicted state 18 | P0 : predicted covariance 19 | 20 | F : factor of real value to previous real value 21 | Q : measurement noise 22 | H : factor of measured value to real value 23 | R : environment noise 24 | 25 | */ 26 | double q = 0.4; 27 | double r = 10; 28 | double f = 1.0; 29 | double h = 1.0; 30 | 31 | kX = makeKalmanFilter (q, r, f, h); 32 | kY = makeKalmanFilter (q, r, f, h); 33 | kZ = makeKalmanFilter (q, r, f, h); 34 | } 35 | 36 | 37 | public Vector3 Update (Vector3 current) 38 | { 39 | kX.Correct (current.x); 40 | kY.Correct (current.y); 41 | kZ.Correct (current.z); 42 | 43 | Vector3 filtered = new Vector3 ( 44 | (float)kX.State, 45 | (float)kY.State, 46 | (float)kZ.State 47 | ); 48 | return filtered; 49 | } 50 | 51 | public void Dispose () 52 | { 53 | 54 | } 55 | 56 | #region Privates 57 | KalmanFilterSimple1D makeKalmanFilter (double q, double r, double f, double h) 58 | { 59 | var filter = new KalmanFilterSimple1D (q,r,f,h); 60 | // set initial value 61 | filter.SetState (500,5.0); 62 | return filter; 63 | } 64 | #endregion 65 | 66 | 67 | } 68 | } -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/SimpleKalmanWrapper.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f033f67a41a024d229128c3a70fb7da2 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Simulate.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 9fa6a9aa00f584246a5456630ca1c4f4 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Simulate/GuassianRandom.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace Kalman.Simulate 4 | { 5 | public sealed class GuassianRandom 6 | { 7 | private bool _hasDeviate; 8 | private double _storedDeviate; 9 | private static Random _random; 10 | 11 | public GuassianRandom() 12 | { 13 | _random = _random ?? new Random(); 14 | } 15 | 16 | /// 17 | /// Obtains normally (Gaussian) distributed random numbers, using the Box-Muller 18 | /// transformation. This transformation takes two uniformly distributed deviates 19 | /// within the unit circle, and transforms them into two independently 20 | /// distributed normal deviates. 21 | /// 22 | /// The mean of the distribution. Default is zero. 23 | /// The standard deviation of the distribution. Default is one. 24 | /// 25 | public double NextGuassian(double mu = 0, double sigma = 1) 26 | { 27 | if (sigma <= 0) 28 | throw new ArgumentOutOfRangeException("sigma", "Must be greater than zero."); 29 | 30 | if (_hasDeviate) 31 | { 32 | _hasDeviate = false; 33 | return _storedDeviate * sigma + mu; 34 | } 35 | 36 | double v1, v2, rSquared; 37 | do 38 | { 39 | // two random values between -1.0 and 1.0 40 | v1 = 2 * _random.NextDouble() - 1; 41 | v2 = 2 * _random.NextDouble() - 1; 42 | rSquared = v1 * v1 + v2 * v2; 43 | // ensure within the unit circle 44 | } while (rSquared >= 1 || rSquared == 0); 45 | 46 | // calculate polar tranformation for each deviate 47 | var polar = Math.Sqrt(-2 * Math.Log(rSquared) / rSquared); 48 | // store first deviate 49 | _storedDeviate = v2 * polar; 50 | _hasDeviate = true; 51 | // return second deviate 52 | return v1 * polar * sigma + mu; 53 | } 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Plugins/Kalman/Simulate/GuassianRandom.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 9ae5ed107b5b24a7ca873491df710899 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Scenes.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 166bfa6ad239b41f6b60247546999e88 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Scenes/Test.unity: -------------------------------------------------------------------------------- 1 | %YAML 1.1 2 | %TAG !u! tag:unity3d.com,2011: 3 | --- !u!29 &1 4 | SceneSettings: 5 | m_ObjectHideFlags: 0 6 | m_PVSData: 7 | m_QueryMode: 1 8 | m_PVSObjectsArray: [] 9 | m_PVSPortalsArray: [] 10 | m_OcclusionBakeSettings: 11 | viewCellSize: 1 12 | bakeMode: 2 13 | memoryUsage: 10485760 14 | --- !u!104 &2 15 | RenderSettings: 16 | m_Fog: 0 17 | m_FogColor: {r: .5, g: .5, b: .5, a: 1} 18 | m_FogMode: 3 19 | m_FogDensity: .00999999978 20 | m_LinearFogStart: 0 21 | m_LinearFogEnd: 300 22 | m_AmbientLight: {r: .200000003, g: .200000003, b: .200000003, a: 1} 23 | m_SkyboxMaterial: {fileID: 0} 24 | m_HaloStrength: .5 25 | m_FlareStrength: 1 26 | m_HaloTexture: {fileID: 0} 27 | m_SpotCookie: {fileID: 0} 28 | m_ObjectHideFlags: 0 29 | --- !u!127 &3 30 | GameManager: 31 | m_ObjectHideFlags: 0 32 | --- !u!157 &4 33 | LightmapSettings: 34 | m_ObjectHideFlags: 0 35 | m_LightProbes: {fileID: 0} 36 | m_Lightmaps: [] 37 | m_LightmapsMode: 1 38 | m_BakedColorSpace: 0 39 | m_UseDualLightmapsInForward: 0 40 | m_LightmapEditorSettings: 41 | m_Resolution: 50 42 | m_LastUsedResolution: 0 43 | m_TextureWidth: 1024 44 | m_TextureHeight: 1024 45 | m_BounceBoost: 1 46 | m_BounceIntensity: 1 47 | m_SkyLightColor: {r: .860000014, g: .930000007, b: 1, a: 1} 48 | m_SkyLightIntensity: 0 49 | m_Quality: 0 50 | m_Bounces: 1 51 | m_FinalGatherRays: 1000 52 | m_FinalGatherContrastThreshold: .0500000007 53 | m_FinalGatherGradientThreshold: 0 54 | m_FinalGatherInterpolationPoints: 15 55 | m_AOAmount: 0 56 | m_AOMaxDistance: .100000001 57 | m_AOContrast: 1 58 | m_LODSurfaceMappingDistance: 1 59 | m_Padding: 0 60 | m_TextureCompression: 0 61 | m_LockAtlas: 0 62 | --- !u!196 &5 63 | NavMeshSettings: 64 | m_ObjectHideFlags: 0 65 | m_BuildSettings: 66 | agentRadius: .5 67 | agentHeight: 2 68 | agentSlope: 45 69 | agentClimb: .400000006 70 | ledgeDropHeight: 0 71 | maxJumpAcrossDistance: 0 72 | accuratePlacement: 0 73 | minRegionArea: 2 74 | widthInaccuracy: 16.666666 75 | heightInaccuracy: 10 76 | m_NavMesh: {fileID: 0} 77 | --- !u!1 &167851558 78 | GameObject: 79 | m_ObjectHideFlags: 0 80 | m_PrefabParentObject: {fileID: 0} 81 | m_PrefabInternal: {fileID: 0} 82 | serializedVersion: 4 83 | m_Component: 84 | - 4: {fileID: 167851559} 85 | - 114: {fileID: 167851560} 86 | m_Layer: 0 87 | m_Name: Controller 88 | m_TagString: Untagged 89 | m_Icon: {fileID: 0} 90 | m_NavMeshLayer: 0 91 | m_StaticEditorFlags: 0 92 | m_IsActive: 1 93 | --- !u!4 &167851559 94 | Transform: 95 | m_ObjectHideFlags: 0 96 | m_PrefabParentObject: {fileID: 0} 97 | m_PrefabInternal: {fileID: 0} 98 | m_GameObject: {fileID: 167851558} 99 | m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} 100 | m_LocalPosition: {x: 0, y: 0, z: 0} 101 | m_LocalScale: {x: 1, y: 1, z: 1} 102 | m_Children: [] 103 | m_Father: {fileID: 0} 104 | --- !u!114 &167851560 105 | MonoBehaviour: 106 | m_ObjectHideFlags: 0 107 | m_PrefabParentObject: {fileID: 0} 108 | m_PrefabInternal: {fileID: 0} 109 | m_GameObject: {fileID: 167851558} 110 | m_Enabled: 1 111 | m_EditorHideFlags: 0 112 | m_Script: {fileID: 11500000, guid: 43559b68ce8964f32972ec31d3f2b1d9, type: 3} 113 | m_Name: 114 | cam: {fileID: 0} 115 | mouseCube: {fileID: 1281102443} 116 | filterdCube: {fileID: 1335543171} 117 | --- !u!1 &319311665 118 | GameObject: 119 | m_ObjectHideFlags: 0 120 | m_PrefabParentObject: {fileID: 0} 121 | m_PrefabInternal: {fileID: 0} 122 | serializedVersion: 4 123 | m_Component: 124 | - 4: {fileID: 319311667} 125 | - 108: {fileID: 319311666} 126 | m_Layer: 0 127 | m_Name: Directional light 128 | m_TagString: Untagged 129 | m_Icon: {fileID: 0} 130 | m_NavMeshLayer: 0 131 | m_StaticEditorFlags: 0 132 | m_IsActive: 1 133 | --- !u!108 &319311666 134 | Light: 135 | m_ObjectHideFlags: 0 136 | m_PrefabParentObject: {fileID: 0} 137 | m_PrefabInternal: {fileID: 0} 138 | m_GameObject: {fileID: 319311665} 139 | m_Enabled: 1 140 | serializedVersion: 3 141 | m_Type: 1 142 | m_Color: {r: 1, g: 1, b: 1, a: 1} 143 | m_Intensity: .5 144 | m_Range: 10 145 | m_SpotAngle: 30 146 | m_CookieSize: 10 147 | m_Shadows: 148 | m_Type: 0 149 | m_Resolution: -1 150 | m_Strength: 1 151 | m_Bias: .0500000007 152 | m_Softness: 4 153 | m_SoftnessFade: 1 154 | m_Cookie: {fileID: 0} 155 | m_DrawHalo: 0 156 | m_ActuallyLightmapped: 0 157 | m_Flare: {fileID: 0} 158 | m_RenderMode: 0 159 | m_CullingMask: 160 | serializedVersion: 2 161 | m_Bits: 4294967295 162 | m_Lightmapping: 1 163 | m_ShadowSamples: 1 164 | m_ShadowRadius: 0 165 | m_ShadowAngle: 0 166 | m_IndirectIntensity: 1 167 | m_AreaSize: {x: 1, y: 1} 168 | --- !u!4 &319311667 169 | Transform: 170 | m_ObjectHideFlags: 0 171 | m_PrefabParentObject: {fileID: 0} 172 | m_PrefabInternal: {fileID: 0} 173 | m_GameObject: {fileID: 319311665} 174 | m_LocalRotation: {x: .408217937, y: -.234569728, z: .109381676, w: .875426054} 175 | m_LocalPosition: {x: 14.9893427, y: -10.1800756, z: -5.35489273} 176 | m_LocalScale: {x: 1, y: 1, z: 1} 177 | m_Children: [] 178 | m_Father: {fileID: 0} 179 | --- !u!1 &1261321925 180 | GameObject: 181 | m_ObjectHideFlags: 0 182 | m_PrefabParentObject: {fileID: 0} 183 | m_PrefabInternal: {fileID: 0} 184 | serializedVersion: 4 185 | m_Component: 186 | - 4: {fileID: 1261321930} 187 | - 20: {fileID: 1261321929} 188 | - 92: {fileID: 1261321928} 189 | - 124: {fileID: 1261321927} 190 | - 81: {fileID: 1261321926} 191 | m_Layer: 0 192 | m_Name: Main Camera 193 | m_TagString: MainCamera 194 | m_Icon: {fileID: 0} 195 | m_NavMeshLayer: 0 196 | m_StaticEditorFlags: 0 197 | m_IsActive: 1 198 | --- !u!81 &1261321926 199 | AudioListener: 200 | m_ObjectHideFlags: 0 201 | m_PrefabParentObject: {fileID: 0} 202 | m_PrefabInternal: {fileID: 0} 203 | m_GameObject: {fileID: 1261321925} 204 | m_Enabled: 1 205 | --- !u!124 &1261321927 206 | Behaviour: 207 | m_ObjectHideFlags: 0 208 | m_PrefabParentObject: {fileID: 0} 209 | m_PrefabInternal: {fileID: 0} 210 | m_GameObject: {fileID: 1261321925} 211 | m_Enabled: 1 212 | --- !u!92 &1261321928 213 | Behaviour: 214 | m_ObjectHideFlags: 0 215 | m_PrefabParentObject: {fileID: 0} 216 | m_PrefabInternal: {fileID: 0} 217 | m_GameObject: {fileID: 1261321925} 218 | m_Enabled: 1 219 | --- !u!20 &1261321929 220 | Camera: 221 | m_ObjectHideFlags: 0 222 | m_PrefabParentObject: {fileID: 0} 223 | m_PrefabInternal: {fileID: 0} 224 | m_GameObject: {fileID: 1261321925} 225 | m_Enabled: 1 226 | serializedVersion: 2 227 | m_ClearFlags: 1 228 | m_BackGroundColor: {r: .23880595, g: .23880595, b: .23880595, a: .0196078438} 229 | m_NormalizedViewPortRect: 230 | serializedVersion: 2 231 | x: 0 232 | y: 0 233 | width: 1 234 | height: 1 235 | near clip plane: .300000012 236 | far clip plane: 1000 237 | field of view: 60 238 | orthographic: 0 239 | orthographic size: 100 240 | m_Depth: -1 241 | m_CullingMask: 242 | serializedVersion: 2 243 | m_Bits: 4294967295 244 | m_RenderingPath: -1 245 | m_TargetTexture: {fileID: 0} 246 | m_HDR: 0 247 | --- !u!4 &1261321930 248 | Transform: 249 | m_ObjectHideFlags: 0 250 | m_PrefabParentObject: {fileID: 0} 251 | m_PrefabInternal: {fileID: 0} 252 | m_GameObject: {fileID: 1261321925} 253 | m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} 254 | m_LocalPosition: {x: 0, y: 1, z: -10} 255 | m_LocalScale: {x: 1, y: 1, z: 1} 256 | m_Children: [] 257 | m_Father: {fileID: 0} 258 | --- !u!1 &1281102438 259 | GameObject: 260 | m_ObjectHideFlags: 0 261 | m_PrefabParentObject: {fileID: 0} 262 | m_PrefabInternal: {fileID: 0} 263 | serializedVersion: 4 264 | m_Component: 265 | - 4: {fileID: 1281102443} 266 | - 33: {fileID: 1281102442} 267 | - 65: {fileID: 1281102441} 268 | - 23: {fileID: 1281102440} 269 | - 96: {fileID: 1281102439} 270 | m_Layer: 0 271 | m_Name: MouseCube 272 | m_TagString: Untagged 273 | m_Icon: {fileID: 0} 274 | m_NavMeshLayer: 0 275 | m_StaticEditorFlags: 0 276 | m_IsActive: 1 277 | --- !u!96 &1281102439 278 | TrailRenderer: 279 | m_ObjectHideFlags: 0 280 | m_PrefabParentObject: {fileID: 0} 281 | m_PrefabInternal: {fileID: 0} 282 | m_GameObject: {fileID: 1281102438} 283 | m_Enabled: 1 284 | m_CastShadows: 1 285 | m_ReceiveShadows: 1 286 | m_LightmapIndex: 255 287 | m_LightmapTilingOffset: {x: 1, y: 1, z: 0, w: 0} 288 | m_Materials: 289 | - {fileID: 2100000, guid: 64c943997ccef444e9b4ea7da414fcfa, type: 2} 290 | m_SubsetIndices: 291 | m_StaticBatchRoot: {fileID: 0} 292 | m_UseLightProbes: 0 293 | m_LightProbeAnchor: {fileID: 0} 294 | m_ScaleInLightmap: 1 295 | m_Time: 1 296 | m_StartWidth: .5 297 | m_EndWidth: .100000001 298 | m_Colors: 299 | m_Color[0]: 300 | serializedVersion: 2 301 | rgba: 4294967295 302 | m_Color[1]: 303 | serializedVersion: 2 304 | rgba: 3422552063 305 | m_Color[2]: 306 | serializedVersion: 2 307 | rgba: 2365587455 308 | m_Color[3]: 309 | serializedVersion: 2 310 | rgba: 1459617791 311 | m_Color[4]: 312 | serializedVersion: 2 313 | rgba: 16777215 314 | m_MinVertexDistance: .00999999978 315 | m_Autodestruct: 0 316 | --- !u!23 &1281102440 317 | Renderer: 318 | m_ObjectHideFlags: 0 319 | m_PrefabParentObject: {fileID: 0} 320 | m_PrefabInternal: {fileID: 0} 321 | m_GameObject: {fileID: 1281102438} 322 | m_Enabled: 1 323 | m_CastShadows: 1 324 | m_ReceiveShadows: 1 325 | m_LightmapIndex: 255 326 | m_LightmapTilingOffset: {x: 1, y: 1, z: 0, w: 0} 327 | m_Materials: 328 | - {fileID: 2100000, guid: f3398b30a088c4bee950f599c6249b3a, type: 2} 329 | m_SubsetIndices: 330 | m_StaticBatchRoot: {fileID: 0} 331 | m_UseLightProbes: 0 332 | m_LightProbeAnchor: {fileID: 0} 333 | m_ScaleInLightmap: 1 334 | --- !u!65 &1281102441 335 | BoxCollider: 336 | m_ObjectHideFlags: 0 337 | m_PrefabParentObject: {fileID: 0} 338 | m_PrefabInternal: {fileID: 0} 339 | m_GameObject: {fileID: 1281102438} 340 | m_Material: {fileID: 0} 341 | m_IsTrigger: 0 342 | m_Enabled: 1 343 | serializedVersion: 2 344 | m_Size: {x: 1, y: 1, z: 1} 345 | m_Center: {x: 0, y: 0, z: 0} 346 | --- !u!33 &1281102442 347 | MeshFilter: 348 | m_ObjectHideFlags: 0 349 | m_PrefabParentObject: {fileID: 0} 350 | m_PrefabInternal: {fileID: 0} 351 | m_GameObject: {fileID: 1281102438} 352 | m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0} 353 | --- !u!4 &1281102443 354 | Transform: 355 | m_ObjectHideFlags: 0 356 | m_PrefabParentObject: {fileID: 0} 357 | m_PrefabInternal: {fileID: 0} 358 | m_GameObject: {fileID: 1281102438} 359 | m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} 360 | m_LocalPosition: {x: .711853027, y: 0, z: 0} 361 | m_LocalScale: {x: 1, y: 1, z: 1} 362 | m_Children: [] 363 | m_Father: {fileID: 0} 364 | --- !u!1 &1335543167 365 | GameObject: 366 | m_ObjectHideFlags: 0 367 | m_PrefabParentObject: {fileID: 0} 368 | m_PrefabInternal: {fileID: 0} 369 | serializedVersion: 4 370 | m_Component: 371 | - 4: {fileID: 1335543171} 372 | - 33: {fileID: 1335543170} 373 | - 65: {fileID: 1335543169} 374 | - 23: {fileID: 1335543168} 375 | - 96: {fileID: 1335543172} 376 | m_Layer: 0 377 | m_Name: FilteredCube 378 | m_TagString: Untagged 379 | m_Icon: {fileID: 0} 380 | m_NavMeshLayer: 0 381 | m_StaticEditorFlags: 0 382 | m_IsActive: 1 383 | --- !u!23 &1335543168 384 | Renderer: 385 | m_ObjectHideFlags: 0 386 | m_PrefabParentObject: {fileID: 0} 387 | m_PrefabInternal: {fileID: 0} 388 | m_GameObject: {fileID: 1335543167} 389 | m_Enabled: 1 390 | m_CastShadows: 1 391 | m_ReceiveShadows: 1 392 | m_LightmapIndex: 255 393 | m_LightmapTilingOffset: {x: 1, y: 1, z: 0, w: 0} 394 | m_Materials: 395 | - {fileID: 2100000, guid: 80e63ac7dbfda43e7a248e077185dfff, type: 2} 396 | m_SubsetIndices: 397 | m_StaticBatchRoot: {fileID: 0} 398 | m_UseLightProbes: 0 399 | m_LightProbeAnchor: {fileID: 0} 400 | m_ScaleInLightmap: 1 401 | --- !u!65 &1335543169 402 | BoxCollider: 403 | m_ObjectHideFlags: 0 404 | m_PrefabParentObject: {fileID: 0} 405 | m_PrefabInternal: {fileID: 0} 406 | m_GameObject: {fileID: 1335543167} 407 | m_Material: {fileID: 0} 408 | m_IsTrigger: 0 409 | m_Enabled: 1 410 | serializedVersion: 2 411 | m_Size: {x: 1, y: 1, z: 1} 412 | m_Center: {x: 0, y: 0, z: 0} 413 | --- !u!33 &1335543170 414 | MeshFilter: 415 | m_ObjectHideFlags: 0 416 | m_PrefabParentObject: {fileID: 0} 417 | m_PrefabInternal: {fileID: 0} 418 | m_GameObject: {fileID: 1335543167} 419 | m_Mesh: {fileID: 10202, guid: 0000000000000000e000000000000000, type: 0} 420 | --- !u!4 &1335543171 421 | Transform: 422 | m_ObjectHideFlags: 0 423 | m_PrefabParentObject: {fileID: 0} 424 | m_PrefabInternal: {fileID: 0} 425 | m_GameObject: {fileID: 1335543167} 426 | m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} 427 | m_LocalPosition: {x: 3.16434288, y: 0, z: 0} 428 | m_LocalScale: {x: 1, y: 1, z: 1} 429 | m_Children: [] 430 | m_Father: {fileID: 0} 431 | --- !u!96 &1335543172 432 | TrailRenderer: 433 | m_ObjectHideFlags: 0 434 | m_PrefabParentObject: {fileID: 0} 435 | m_PrefabInternal: {fileID: 0} 436 | m_GameObject: {fileID: 1335543167} 437 | m_Enabled: 1 438 | m_CastShadows: 1 439 | m_ReceiveShadows: 1 440 | m_LightmapIndex: 255 441 | m_LightmapTilingOffset: {x: 1, y: 1, z: 0, w: 0} 442 | m_Materials: 443 | - {fileID: 2100000, guid: 371398fc62d5c43808b4a4028d9c6112, type: 2} 444 | m_SubsetIndices: 445 | m_StaticBatchRoot: {fileID: 0} 446 | m_UseLightProbes: 0 447 | m_LightProbeAnchor: {fileID: 0} 448 | m_ScaleInLightmap: 1 449 | m_Time: 1 450 | m_StartWidth: .5 451 | m_EndWidth: .100000001 452 | m_Colors: 453 | m_Color[0]: 454 | serializedVersion: 2 455 | rgba: 4294967295 456 | m_Color[1]: 457 | serializedVersion: 2 458 | rgba: 3422552063 459 | m_Color[2]: 460 | serializedVersion: 2 461 | rgba: 2365587455 462 | m_Color[3]: 463 | serializedVersion: 2 464 | rgba: 1459617791 465 | m_Color[4]: 466 | serializedVersion: 2 467 | rgba: 16777215 468 | m_MinVertexDistance: .00999999978 469 | m_Autodestruct: 0 470 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Scenes/Test.unity.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: ad35ee1c7f95e443fa58ab8c5253a8dd 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Scripts.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: a12c48ca8af954fda9d359f37756ad6d 3 | DefaultImporter: 4 | userData: 5 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Scripts/Test.cs: -------------------------------------------------------------------------------- 1 | using UnityEngine; 2 | using Kalman; 3 | 4 | /// 5 | /// Test code 6 | /// 7 | public class Test : MonoBehaviour { 8 | 9 | [SerializeField] 10 | Camera cam; 11 | 12 | [SerializeField] 13 | Transform mouseCube; 14 | 15 | [SerializeField] 16 | Transform filterdCube; 17 | 18 | IKalmanWrapper kalman; 19 | 20 | void Awake () 21 | { 22 | kalman = new MatrixKalmanWrapper (); 23 | //kalman = new SimpleKalmanWrapper (); 24 | } 25 | 26 | void Start () 27 | { 28 | cam = Camera.mainCamera; 29 | } 30 | 31 | // Update is called once per frame 32 | void Update () 33 | { 34 | //Vector3 35 | Vector3 mouse = Input.mousePosition; 36 | mouse.z = 10; 37 | mouseCube.transform.position = cam.ScreenToWorldPoint (mouse); 38 | 39 | filterdCube.transform.position = cam.ScreenToWorldPoint (kalman.Update (mouse)); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /KalmanSample/Assets/Scripts/Test.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 43559b68ce8964f32972ec31d3f2b1d9 3 | MonoImporter: 4 | serializedVersion: 2 5 | defaultReferences: [] 6 | executionOrder: 0 7 | icon: {instanceID: 0} 8 | userData: 9 | -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/AudioManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/AudioManager.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/DynamicsManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/DynamicsManager.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/EditorBuildSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/EditorBuildSettings.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/EditorSettings.asset: -------------------------------------------------------------------------------- 1 | %YAML 1.1 2 | %TAG !u! tag:unity3d.com,2011: 3 | --- !u!159 &1 4 | EditorSettings: 5 | m_ObjectHideFlags: 0 6 | serializedVersion: 2 7 | m_ExternalVersionControlSupport: Meta Files 8 | m_SerializationMode: 2 9 | m_WebSecurityEmulationEnabled: 0 10 | m_WebSecurityEmulationHostUrl: http://www.mydomain.com/mygame.unity3d 11 | -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/InputManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/InputManager.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/NavMeshLayers.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/NavMeshLayers.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/NetworkManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/NetworkManager.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/ProjectSettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/ProjectSettings.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/QualitySettings.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/QualitySettings.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/TagManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/TagManager.asset -------------------------------------------------------------------------------- /KalmanSample/ProjectSettings/TimeManager.asset: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asus4/UnityIMU/c7b5a54b7fc552a1e9f4b315e51083246965814a/KalmanSample/ProjectSettings/TimeManager.asset -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # IMU in Unity Project 2 | 3 | 4 | - KalmanSample 5 | - Kalman filter in Unity 6 | - forked from [https://github.com/justserega/kalman-examples](https://github.com/justserega/kalman-examples) 7 | - Matrix.cs [link](http://blog.ivank.net/lightweight-matrix-class-in-c-strassen-algorithm-lu-decomposition.html) --------------------------------------------------------------------------------