Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

adjustments to algorithm, and split up local log if running for more …

…than an hour.
  • Loading branch information...
commit ba4ce005cd23388643915aa0280aa104cd0c2bdd 1 parent 971f1c3
Marije Baalman authored
View
9 app/res/layout/guesstimator.xml
@@ -2,7 +2,6 @@
<LinearLayout android:id="@+id/LinearLayout01" android:layout_width="fill_parent" android:layout_height="fill_parent" xmlns:android="http://schemas.android.com/apk/res/android" android:orientation="vertical" android:padding="6dp">
<LinearLayout android:id="@+id/LinearLayout02" android:layout_width="fill_parent" android:layout_height="wrap_content" android:orientation="vertical">
-
<TextView android:textAppearance="?android:attr/textAppearanceLarge" android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/MotionStatusTextView" android:text="Still" android:gravity="center_vertical|center_horizontal"></TextView>
<TableLayout android:id="@+id/LinearLayout04" android:layout_width="fill_parent" android:layout_height="wrap_content" android:stretchColumns="1" android:padding="6dp">
<TableRow>
@@ -47,6 +46,7 @@
</TableLayout>
</LinearLayout>
+
<ScrollView android:id="@+id/ScrollView01" android:layout_width="fill_parent" android:layout_height="wrap_content">
@@ -160,7 +160,12 @@
<EditText android:id="@+id/speed_decay" android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_weight="1" android:inputType="numberDecimal" android:text="0.99">
</EditText>
</TableRow>
- </TableLayout>
+ <TableRow>
+ <TextView android:layout_width="wrap_content" android:layout_height="wrap_content" android:id="@+id/MMALabelTextView" android:text="Mean MA"></TextView>
+ <EditText android:id="@+id/mean_coef" android:layout_width="wrap_content" android:layout_height="wrap_content" android:layout_weight="1" android:inputType="numberDecimal" android:text="0.99">
+ </EditText>
+ </TableRow>
+ </TableLayout>
<TextView android:textAppearance="?android:attr/textAppearanceLarge" android:layout_width="fill_parent" android:layout_height="wrap_content" android:id="@+id/SettingLabelTextView" android:text="SERVER" android:gravity="center_vertical|center_horizontal"></TextView>
<TableLayout android:id="@+id/TableLayout3" android:layout_width="fill_parent" android:layout_height="wrap_content" android:stretchColumns="1">
View
24 app/src/com/steim/nescivi/android/gvb/GuesstimateVelocityBetter.java
@@ -723,6 +723,14 @@ public void send_estimate_settings(){
System.out.println("Could not parse " + nfe);
}
+ ed = (EditText) findViewById(R.id.mean_coef);
+ float mean_coef = 0.99f;
+ try {
+ mean_coef = Float.parseFloat(ed.getText().toString());
+ } catch(NumberFormatException nfe) {
+ System.out.println("Could not parse " + nfe);
+ }
+
ed = (EditText) findViewById(R.id.offsetMA);
float offsetma = 0.99f;
try {
@@ -802,6 +810,7 @@ public void send_estimate_settings(){
b.putFloat("mean_weight", mean_weight );
b.putFloat("raw_weight", raw_weight );
b.putFloat("speed_decay", speed_decay );
+ b.putFloat("mean_coef", mean_coef );
b.putFloat("offsetma", offsetma );
b.putInt("signForward", signForward );
@@ -1003,7 +1012,9 @@ public void readPreferences(){
float mean_weight = mPrefs.getFloat("mean_weight", 0.65f );
float raw_weight = mPrefs.getFloat("raw_weight", 0.35f );
-
+
+ float mean_coef = mPrefs.getFloat("mean_coef", 0.99f );
+
float speed_decay = mPrefs.getFloat("speed_decay", 0.99f );
float offsetma = mPrefs.getFloat("offsetma", 0.99f );
@@ -1091,6 +1102,8 @@ public void readPreferences(){
ed = (EditText) findViewById(R.id.speed_decay );
ed.setText( Float.toString( speed_decay ) );
+ ed = (EditText) findViewById(R.id.mean_coef );
+ ed.setText( Float.toString( mean_coef ) );
ed = (EditText) findViewById(R.id.offsetMA);
ed.setText( Float.toString( offsetma ) );
ed = (EditText) findViewById(R.id.editWindow);
@@ -1251,6 +1264,13 @@ public void storePreferences(){
} catch(NumberFormatException nfe) {
System.out.println("Could not parse " + nfe);
}
+ ed = (EditText) findViewById(R.id.mean_coef);
+ float mean_coef = 0.99f;
+ try {
+ mean_coef = Float.parseFloat(ed.getText().toString());
+ } catch(NumberFormatException nfe) {
+ System.out.println("Could not parse " + nfe);
+ }
ed = (EditText) findViewById(R.id.offsetMA);
float offsetma = 0.99f;
@@ -1362,6 +1382,8 @@ public void storePreferences(){
mPrefsEdit.putFloat("deceleration_forward", dec_forward );
mPrefsEdit.putFloat("deceleration_mean", dec_mean );
mPrefsEdit.putFloat("speed_decay", speed_decay );
+ mPrefsEdit.putFloat("mean_coef", mean_coef );
+
mPrefsEdit.putFloat("still_forward", still_forward );
mPrefsEdit.putFloat("still_side", still_side );
mPrefsEdit.putFloat("motion_forward", motion_forward );
View
57 app/src/com/steim/nescivi/android/gvb/VelocityEstimator.java
@@ -36,6 +36,8 @@
import java.io.OutputStream;
import java.io.PrintWriter;
import java.io.IOException;
+import java.lang.Math;
+
//import android.hardware.SensorEventListener;
@@ -71,6 +73,8 @@
static final int MSG_GUI_UPDATE_MSG = 15;
static final int MSG_GPS_LOC = 16;
+
+ private int mWrittenLines = 0;
// static final int MSG_REGISTER_VS_SRV = 5;
// static final int MSG_UNREGISTER_VS_SRV = 6;
@@ -123,7 +127,10 @@
private float [][] mCurrentStats = { { (float) 0.0, (float) 0.0, (float) 0.0 }, { (float) 0.0, (float) 0.0, (float) 0.0 } };
//float stats[][] = new float[2][3];
- private float mSpeed = (float) 0.0;
+ private double mSpeed = 0.0;
+ private float[] mamean = { (float) 0.0, (float) 0.0, (float) 0.0 };
+ private float[] mameanOff = { (float) 0.0, (float) 0.0, (float) 0.0 };
+ private float mameanCoef = 0.99f;
private int mForward = 1;
private int mSideways = 0;
@@ -199,6 +206,7 @@ public void handleMessage(Message msg)
threshold_motion_side = msg.getData().getFloat("motion_side");
mean_weight = msg.getData().getFloat("mean_weight");
raw_weight = msg.getData().getFloat("raw_weight");
+ mameanCoef = msg.getData().getFloat("mean_coef");
speed_decay = msg.getData().getFloat("speed_decay");
macoef = msg.getData().getFloat("offsetma");
forwardsign = msg.getData().getInt("signForward");
@@ -281,14 +289,17 @@ public void run() {
float [] currentGPSReadings = mGPSListener.getCurrentValues();
float [] logdata = {
(float) mState,
- mSpeed, mSpeed * 3.6f,
+ (float) mSpeed, (float) mSpeed * 3.6f,
mCurrentStats[0][0], mCurrentStats[1][0],
mCurrentStats[0][1], mCurrentStats[1][1],
mCurrentStats[0][2], mCurrentStats[1][2],
mOffsets[0],mOffsets[1],mOffsets[2],
//mStillTime,
currentReadings[0],currentReadings[1], currentReadings[2],
- currentGPSReadings[0],currentGPSReadings[1]
+ currentGPSReadings[0],currentGPSReadings[1],
+ mameanOff[0], mamean[0],
+ mameanOff[1], mamean[1],
+ mameanOff[2], mamean[2]
};
writeLogData( logdata );
}
@@ -482,6 +493,8 @@ private void readPreferences(){
mean_weight = mPrefs.getFloat("mean_weight", 0.65f );
raw_weight = mPrefs.getFloat("raw_weight", 0.35f );
+
+ mameanCoef = mPrefs.getFloat("mean_coef", 0.99f );
speed_decay = mPrefs.getFloat("speed_decay", 0.99f );
macoef = mPrefs.getFloat("offsetma", 0.99f );
@@ -692,7 +705,7 @@ private void send_gui_update_msg(){
Bundle b = new Bundle();
Message msg = Message.obtain(null, MSG_GUI_UPDATE_MSG );
b.putInt("motion", mState );
- b.putFloat("speed", mSpeed );
+ b.putFloat("speed", (float) mSpeed );
synchronized( this ){
b.putFloat("facc_mean", this.mCurrentStats[0][0] );
@@ -798,22 +811,26 @@ public void updateVelocityMeasurement(){
//mCurrentStats = mBuffer.getStats();
float [][] curStats = this.mListener.getCurrentStats();
+ for ( int axis = 0; axis < 3; axis++ ){
+ mamean[axis] = mamean[axis] * mameanCoef + curStats[0][axis]*(1.0f-mameanCoef);
+ }
+
// reset offsets when we are in still:
if ( this.mState == 0 ) {
for ( int axis = 0; axis < 3; axis++ ){
if ( curStats[0][axis] < 20.f ){ // just to make sure we're not killed by NaN's
- this.mOffsets[axis] = this.mOffsets[axis] * macoef + curStats[0][axis] * (1-macoef);
+ this.mOffsets[axis] = this.mOffsets[axis] * macoef + mamean[axis] * (1-macoef);
}
}
}
// substract offset from mean
for ( int axis = 0; axis < 3; axis++ ){
- curStats[0][axis] = curStats[0][axis] - this.mOffsets[axis];
+ mameanOff[axis] = mamean[axis] - this.mOffsets[axis];
}
// correct sign for forward axis
- curStats[0][0] = curStats[0][0] * forwardsign;
+ mameanOff[0] = mameanOff[0] * forwardsign;
currentReadings[0] = (currentReadings[0] - this.mOffsets[0]) * forwardsign;
// determine state
@@ -824,10 +841,10 @@ public void updateVelocityMeasurement(){
if ( curStats[1][0] < threshold_still_forward && curStats[1][1] < threshold_still_side ){
this.mState = 0; // still
}
- if ( curStats[1][0] > threshold_acceleration_forward && curStats[0][0] > threshold_acceleration_mean ){
+ if ( curStats[1][0] > threshold_acceleration_forward && mameanOff[0] > threshold_acceleration_mean ){
this.mState = 2; // accelerating
}
- if ( curStats[1][0] > threshold_deceleration_forward && curStats[0][0] < threshold_deceleration_mean ){
+ if ( curStats[1][0] > threshold_deceleration_forward && mameanOff[0] < threshold_deceleration_mean ){
this.mState = 3; // decelerating
}
@@ -870,13 +887,20 @@ public void updateVelocityMeasurement(){
}
}
*/
-
+
// calculate forward speed
+ // limit speed increase when getting faster:
+ double limiterFactor = Math.exp( -1. * this.mSpeed * Math.PI / 10. );
+ double deltaspeed = (mean_weight*mameanOff[0] + raw_weight*currentReadings[0]) * this.mDeltaTime * 0.001;
+ if ( deltaspeed > 0 ){
+ this.mSpeed += deltaspeed * limiterFactor;
+ } else {
+ this.mSpeed += deltaspeed;
+ }
if ( this.mState == 0 ){
this.mSpeed = this.mSpeed * speed_decay;
- } else {
- this.mSpeed += ( mean_weight*curStats[0][0] + raw_weight*currentReadings[0]) * this.mDeltaTime * 0.001;
}
+ this.mSpeed = Math.max( mSpeed, 0.0 );
/*
switch (this.mState){
@@ -896,7 +920,8 @@ public void updateVelocityMeasurement(){
*/
synchronized( this ){
for ( int axis = 0; axis < 3; axis++ ){
- mCurrentStats[0][axis] = curStats[0][axis];
+// mCurrentStats[0][axis] = curStats[0][axis];
+ mCurrentStats[0][axis] = mameanOff[axis];
mCurrentStats[1][axis] = curStats[1][axis];
}
}
@@ -951,6 +976,7 @@ public void createLocalLog() {
if ( mMakeLocalLog ){
try {
mLocalLog = new SensorOutputWriter(SensorOutputWriter.TYPE_GVB);
+ mWrittenLines = 0;
mWritingLocalLog = true;
} catch (StorageErrorException ex) {
ex.printStackTrace();
@@ -962,6 +988,11 @@ public void writeLogData( float[] values ){
if ( mWritingLocalLog ){
try {
mLocalLog.writeReadings(values);
+ mWrittenLines++;
+ if ( mWrittenLines > 360000 ){ // start a new file each hour
+ closeLocalLog();
+ createLocalLog();
+ }
} catch (StorageErrorException ex) {
ex.printStackTrace();
}
View
142 tools/plotdata/plotloggvb.py
@@ -117,7 +117,7 @@ def motion_state_machine( meanF, stdF, meanS, stdS, deltaT ):
global still_count
global motion_state_index
- if stdF > 0.1 and stdS > 0.1:
+ if stdF > 0.1 and stdS > 0.04:
motion_state = 'motion'
motion_state_index = 1.
if stdF < 0.04 and stdS < 0.04:
@@ -140,13 +140,29 @@ def calculate_speed( meanF, rawF, deltaT ):
global speed
global offset
global motion_state
+ global deltaspeed
#global motion_still_count
- if motion_state == 'still':
- speed = speed * 0.99
- offset = 0.99*offset + 0.01*meanF
- else: #moving
- #speed += ( (rawF+meanF)/2. - offset ) * deltaT * 0.001 * 3.6
- speed += (0.35*(rawF- offset) + 0.65*(meanF- offset) ) * deltaT * 0.001 * 3.6
+ #if motion_state == 'still':
+ #speed = speed * 0.99
+ #offset = 0.99*offset + 0.01*meanF
+ #else: #moving
+ #speed += ( (rawF+meanF)/2. - offset ) * deltaT * 0.001 * 3.6
+ #limiter = 1.
+ #deltaspeed = (0.05*(rawF- offset) + 0.85*(meanF- offset) ) * deltaT * 0.001 * 3.6
+ deltaspeed = meanF * deltaT * 0.001 * 3.6
+ if deltaspeed > 0:
+ if motion_state == 'still':
+ speed = speed * 0.95
+ limiter = numpy.exp( -max(speed,0.)*math.pi/30. )
+ speed += deltaspeed * limiter
+ else:
+ if motion_state == 'still':
+ speed = speed * 0.95
+ speed += deltaspeed*4
+ #print motion_state, speed, deltaspeed
+ speed = max( speed, 0. )
+
+ #speed = max( speed, 0. )
#speed = speed * 3.6 # km/h
#def substract_offset( value, offset ):
@@ -187,7 +203,7 @@ def openFile( filename ):
#plotZ = makeplot( mplot[0], mplot[3], "Z" )
#ContainerHor(plotX,plotY,plotZ).configure_traits()
- #overallmean = numpy.mean( mplot[1:4], -1 )
+ overallmean = numpy.mean( mplot[1:4], -1 )
##print overallmean
##print overallmean.shape
@@ -198,36 +214,63 @@ def openFile( filename ):
#meanDT = numpy.mean( deltaStride, -1 )
#stdDT = numpy.std( deltaStride, -1 )
- #mplotCorr = mplot[1:4] - overallmean[:, numpy.newaxis]
+ mplotCorr = mplot[1:4] - overallmean[:, numpy.newaxis]
##mplotCorr = map( x - overallmean , mplot[1:4] )
##print mplotCorr.shape
##mplotStride = rolling_window( mplot[1:4], 200 )
- #mplotStride = rolling_window( mplotCorr, 200 )
+ mplotStride = rolling_window( mplotCorr, 200 )
##print mplotStride.shape
##print mplotStride.transpose(1,0).shape
- #means = numpy.mean( mplotStride, -1 )
- ##print means.shape
+ means = numpy.mean( mplotStride, -1 )
+ print means.shape
- #stds = numpy.std( mplotStride, -1 )
+ stds = numpy.std( mplotStride, -1 )
##print stds.shape
##print meanDT.size
### calculate motion
speeds = []
+ deltaspeeds = []
+ offsets = []
motions = []
- for i in range( 1, mplot[4].size ):
+ mameans = []
+ precision = []
+ #mam = mplot[4][0]
+ mam = means[0][0]
+ avgspeed = []
+ #for i in range( 1, mplot[4].size ):
+ for i in range( 0, means[0].size - 1 ):
+ #print i
+ #mam = mam*0.99 + mplot[4][i]*0.01
+ mam = mam*0.99 + (means[0][i]-offset)*0.01
+ mameans.append( mam )
##print i
- motion_state_machine( mplot[4][i], mplot[5][i], mplot[6][i], mplot[7][i], 5 )
+ #motion_state_machine( mplot[4][i], mplot[5][i], mplot[6][i], mplot[7][i], 5 )
+ #motion_state_machine( mam, mplot[5][i], mplot[6][i], mplot[7][i], 5 )
+ motion_state_machine( mam, stds[0][i], means[1][i], stds[1][i], 10 )
+ if motion_state == 'still':
+ offset = 0.99*offset + 0.01*means[0][i]
#calculate_speed( mplot[4][i], 5 )
- calculate_speed( mplot[4][i], mplot[14][i], 5 )
+ calculate_speed( mam, mplot[13][i], 5 )
+ #calculate_speed( mam, mplot[13][i], 5 )
##calculate_speed( means[1][i], deltatimes[i] )
+ precision.append( numpy.exp( - stds[0][i] * math.pi/2.0 ) )
+ #precision.append( numpy.exp( -max( mplot[5][i], 0. )*math.pi/2.0 ))
speeds.append( speed )
+ offsets.append( offset )
+ deltaspeeds.append( deltaspeed )
motions.append( motion_state_index )
+ avgspeed.append( (speed + mplot[16][i])/ 2. );
##print speed
mspeeds = numpy.asarray( speeds )
+ moffsets = numpy.asarray( offsets )
+ mdspeeds = numpy.asarray( deltaspeeds )
mmotion = numpy.asarray( motions )
+ mmameans = numpy.asarray( mameans )
+ mprecision = numpy.asarray( precision )
+ mavgspeed = numpy.asarray( avgspeed )
@@ -244,36 +287,67 @@ def openFile( filename ):
plotC3 = makeplot2( timeaxis, mspeeds, "speed calc" )
plot1 = makeplot( timeaxis, mplot[1], "motion", -0.5, 3.5 )
- plot2 = makeplot( timeaxis, mplot[2], "speed", -20, 20 )
- plot3 = makeplot( timeaxis, mplot[4], "mean", -2.5, 2.5 )
- plot4 = makeplot( timeaxis, mplot[5], "std", -0.05, 0.6 )
+ plot2 = makeplot2( timeaxis, mplot[2], "speed" )
+
+ plot3 = makeplot2( timeaxis, mplot[4], "mean" )
+ plot4 = makeplot2( timeaxis, mplot[5], "std" )
plot5 = makeplot( timeaxis, mplot[6], "side mean", -2.5, 2.5 )
plot6 = makeplot( timeaxis, mplot[7], "side std", -0.05, 0.6 )
plot7 = makeplot( timeaxis, mplot[8], "grav mean", -2.5, 2.5 )
plot8 = makeplot( timeaxis, mplot[9], "grav std", -0.05, 0.6 )
- plot9 = makeplot( timeaxis, mplot[10], "offset", -0.5, 0.5 )
- plot10 = makeplot( timeaxis, mplot[13], "stilltime", 0, 5 )
- plot11 = makeplot( timeaxis, mplot[14], "acc forward", -3.5, 3.5 )
- plot12 = makeplot( timeaxis, mplot[15], "acc side", -3.5, 3.5 )
- plot13 = makeplot( timeaxis, mplot[16], "acc grav", 6.5, 13.5 )
+ plot9 = makeplot2( timeaxis, mplot[10], "offset" )
+ #plot10 = makeplot( timeaxis, mplot[13], "stilltime", 0, 5 )
+ plot11 = makeplot2( timeaxis, mplot[13], "acc forward" )
+ plot12 = makeplot2( timeaxis, mplot[14], "acc side" )
+ plot13 = makeplot2( timeaxis, mplot[15], "acc grav" )
+ plot14 = makeplot2( timeaxis, mplot[16], "gps speed" )
+ plot15 = makeplot2( timeaxis, mplot[17], "gps precision" )
+
+ #plot16 = makeplot2( timeaxis[1000:2000], mplot[13][500:2000], "acc forward detail" )
+ #plot17 = makeplot2( timeaxis[200:2000], mplot[4][200:2000], "mean forward detail" )
+ #plot18 = makeplot2( timeaxis[200:2000], mplot[5][200:2000], "std forward detail" )
+ #plot19 = makeplot2( timeaxis[1000:2000], mplot[10][500:1500], "offset detail" )
+ #plot20 = makeplot2( timeaxis[200:2000], mmameans[200:2000], "moving average mean" )
+
+ plot21 = makeplot2( timeaxis, mmameans, "moving average mean" )
+ #plot22 = makeplot2( timeaxis[200:2000], mplot[7][200:2000], "std side detail" )
+
+ plot23 = makeplot2( timeaxis, mprecision, "precision" )
+ plot24 = makeplot2( timeaxis, mdspeeds, "deltaspeed" )
+ plot25 = makeplot2( timeaxis, moffsets, "offets calc" )
+
+ plot26 = makeplot2( timeaxis, mavgspeed, "average speed (gps + calc)" )
+
+ ContainerHor4(plot4,plot14,plotC3,plot26).configure_traits()
+
+ ContainerHor4(plotC3,plot24,plot21,plot25).configure_traits()
+
+ ContainerHor4(plot14,plotC3,plot3,plot4).configure_traits()
+ ContainerHor4(plot14,plotC3,plot21,plot4).configure_traits()
+ #ContainerHor4(plotC1,plotC2,plot2,plot3).configure_traits()
+ #ContainerHor4(plotC1,plotC2,plot4,plot6).configure_traits()
- ContainerHor4(plotC1,plotC2,plot1,plot2).configure_traits()
- ContainerHor4(plotC1,plotC2,plot2,plot3).configure_traits()
- ContainerHor4(plotC1,plotC2,plot4,plot6).configure_traits()
+ #ContainerHor4(plot2,plot3,plot9,plot14).configure_traits()
+ #ContainerHor4(plot4,plot2,plot14,plot6).configure_traits()
+ #ContainerHor4(plot1,plot2,plot14,plot15).configure_traits()
+ #ContainerHor4(plotC1,plotC2,plot1,plot2).configure_traits()
#ContainerHor4(plot1,plot2,plot3,plot4).configure_traits()
#ContainerHor4(plot5,plot6,plot7,plot8).configure_traits()
#ContainerHor4(plot2,plot3,plot9,plot10).configure_traits()
#ContainerHor4(plot11,plot12,plot13,plot2).configure_traits()
- #ContainerHor4(plotYm,plotYs,plotXm,plotXs).configure_traits()
- #ContainerHor4(plot3,plot4,plot6,plot8).configure_traits()
- #ContainerHor4(plot11,plot3,plot4,plot2).configure_traits()
+ ##ContainerHor4(plotYm,plotYs,plotXm,plotXs).configure_traits()
+ ##ContainerHor4(plot3,plot4,plot6,plot8).configure_traits()
+ ##ContainerHor4(plot11,plot3,plot4,plot2).configure_traits()
#LinePlot( plot4 ).configure_traits();
#LinePlot( plot6 ).configure_traits();
- #LinePlot( plot8 ).configure_traits();
- #LinePlot( plot11 ).configure_traits();
- #LinePlot( plot13 ).configure_traits();
- LinePlot( plotC3 ).configure_traits();
+ ##LinePlot( plot8 ).configure_traits();
+ ##LinePlot( plot11 ).configure_traits();
+ ##LinePlot( plot13 ).configure_traits();
+ ##LinePlot( plotC3 ).configure_traits();
+ #LinePlot( plot16 ).configure_traits();
+ #LinePlot( plot18 ).configure_traits();
+ #ContainerHor4(plot17,plot18,plot22,plot20).configure_traits()
#ContainerGrid(plotXm,plotYm,plotZm,plotXs,plotYs,plotZs).configure_traits()
Please sign in to comment.
Something went wrong with that request. Please try again.