Calculate height/position from accelerometer/gyro/compass

I have an accelerometer and would like to calculate position/height during movement. There is a code example that can be followed but it needs to be modified for accuracy and filter out noise. Basic idea is to integrate acceleration to get speed and integrate speed to get distance. [login to view URL] I have both raw acceleration/gyro/magnometer data and fused data using either euler angles or quaternions. The movement will be mostly up and down motion and I would like to calculate the height in mm/cm pref. at ~5 cm accuracy. I can provide recorded data of the movement with the given height, I need in return a function in any language (Pref. C/C++/Java/...) given the above input.