Lidar lite distance tolerance issue not within 2cm

I am running the lidar lite at 3.3 volts. The issue I am seeing the raw distances are not with in 2 cm. The delta I see is 6 cm when the object is stationary. I Know I can doing a moving average but I am trying to capture motion distance so I am using the raw data. Would adding a cap to the input voltage help improve the values?

#include <I2Cdev.h>
#include <MPU6050.h>
#include <I2C.h>

#define LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
#define RegisterMeasure 0x00 // Register to write to initiate ranging.
#define MeasureValue 0x04 // Value to initiate ranging.
#define RegisterHighLowB 0x8f // Register to get both High and Low bytes in 1 call.
#define CalibrationRegister 0x13 // The register to set for calibration
#define CalibrationOffsetVlue 0x03 // The calibration offset… see note below.

#define READ 0

unsigned long Serial1data;
int inbyte;

/******************
** Function name: setup
** Descriptions:
*****************/
void setup()
{
Serial1.begin(115200);
I2c.begin(); // Opens & joins the irc bus as master
delay(100); // Waits to make sure everything is powered up before sending or receiving data
I2c.timeOut(50); // Sets a timeout to ensure no locking up of sketch if I2C communication fails
LidarLiteCalibrate();
}

/*****************************
** Function name: loop
** Descriptions: loops runs constantly
*****************************/
void loop()
{
stateMachine();
} //end loop

/**************************
** Function name: readRxData()
** Descriptions:
**************************/
word readRxData()
{
word result = false;
int inByte = Serial1.read();

switch(inByte)
{

case ‘w’:
getStartPosition();
result = READ;
break;

case ‘e’:
getEndPosition();
result = READ;
break;

case ‘g’:
inbyte = inByte;
sendData();
break;

}
return( result );
}

/****************************************************************************
** Function name: stateMachine()
** Descriptions: State Machine
** READ 0
****************************************************************************/
void stateMachine()
{
static word mode = READ; // read is 0

switch ( mode )
{
case READ : //0
mode = readRxData();
break;
}
}

/****************************************************************************
** Function name: getStartPosition()
** Descriptions: get the initial height start position
****************************************************************************/
void getStartPosition()
{
double startPosition= 0;
startPosition = initialize_height();
Serial1.print("#"); //start of data packet
Serial1.print(startPosition);
Serial1.println("~"); //end of data packet
}

/****************************************************************************
** Function name: getEndPosition()
** Descriptions: get the final height end position
****************************************************************************/
void getEndPosition()
{
double endPosition = 0;
endPosition = initialize_height();
Serial1.print("$"); //start of data packet
Serial1.print(endPosition);
Serial1.println("~"); //end of data packet
}

/****************************************************************************
** Function name: initialize_height
** Descriptions: gathers the initial height of the repetition for the algorithm
** to base high and low points off of. pos_first is the current
** smoothed sample,pos_second is the previous smoothed sample etc.
****************************************************************************/
double initialize_height()
{

double initial_height;
double inh = LidarLiteRawDistance();
delay(100);
inh = inh + LidarLiteRawDistance();
delay(100);
inh = inh + LidarLiteRawDistance();
delay(100);
initial_height = inh / 3;

return initial_height;

}

/****************************************************************************
** Function name: sendData()
** Descriptions: transmit the data via ble
****************************************************************************/
void sendData()
{

unsigned long timestamp;
int distance;

// stop sending data when received ‘s’
while(inbyte !=‘s’)
{
inbyte = Serial1.read();

distance = LidarLiteRawDistance();
timestamp = millis();
Serial1.print(":"); //start of data packet
Serial1.print(",");
Serial1.print(timestamp);
Serial1.print(",");
Serial1.print(distance);
Serial1.print(",");
Serial1.println("~"); //end of data packet

}

}

/****************************************************************************
** Function name: LidarLiteCalibrate()
** Descriptions: calibrate the sensor
****************************************************************************/
void LidarLiteCalibrate()
{
// Write 0x04 to register 0x00
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.write(LIDARLite_ADDRESS,CalibrationRegister, CalibrationOffsetVlue); // Write Calibration Offset Value to 0x13
delay(1); // Wait 1 ms to prevent overpolling
}
}

/****************************************************************************
** Function name: LidarLiteRawDistance()
** Descriptions: get the distance in cm.
****************************************************************************/
double LidarLiteRawDistance()
{

// Write 0x04 to register 0x00
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.write(LIDARLite_ADDRESS,RegisterMeasure, MeasureValue); // Write 0x04 to 0x00
delay(1); // Wait 1 ms to prevent overpolling
}

byte distanceArray[2]; // array to store distance bytes from read function

// Read 2byte distance from register 0x8f
nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
nackack = I2c.read(LIDARLite_ADDRESS,RegisterHighLowB, 2, distanceArray); // Read 2 Bytes from LIDAR-Lite Address and store in array
delay(1); // Wait 1 ms to prevent overpolling
}
double distance = (distanceArray[0] << 8) + distanceArray[1]; // Shift high byte [0] 8 to the left and add low byte [1] to create 16-bit int

return distance; // give us this value
}

/****************************************************************************
** Function name: ema_filter
** Descriptions: This function filters the raw data
**
****************************************************************************/
double ema_filter(double current_value)
{
int alpha = 5; ///This is in percentage. Should be between 0-99/
static uint16_t exponential_average = current_value;

exponential_average=(alpha*(uint32_t)current_value + (100 - alpha)*(uint32_t)exponential_average)/100;
return exponential_average;
}

/* ============================================================================
Basic read and write functions for LIDAR-Lite, waits for success message (0 or ACK) before proceeding
===========================================================================*/

// Write a register and wait until it responds with success
void llWriteAndWait(char myAddress, char myValue)
{
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){
nackack = I2c.write(LIDARLite_ADDRESS,myAddress, myValue);
delay(2); // Wait 2 ms to prevent overpolling
}
}

// Read 1-2 bytes from a register and wait until it responds with sucess
byte llReadAndWait(char myAddress, int numOfBytes, byte arrayToSave[2])
{
uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses
while (nackack != 0){
nackack = I2c.read(LIDARLite_ADDRESS,myAddress, numOfBytes, arrayToSave);
delay(2); // Wait 2 ms to prevent overpolling
}
return arrayToSave[2]; // Return array for use in other functions
}

My output is located here:

http://1drv.ms/1Ru0AB0

Please edit your post and add code tags ("</>" button).

The Lidar Lite runs on 5V.