I just did some tests myself, and I ended up with something similar to Aswin.
Test method:
I created a small statistics module:
Code: Select all
package com.kvols.stat;
import static java.lang.Math.sqrt;
public class Stat {
int n;
double sumX, sumY, sumXY, sumXX, sumYY;
public void add(double x, double y) {
n++;
sumX += x;
sumY += y;
sumXY += x * y;
sumXX += x * x;
sumYY += y * y;
}
double getMean() {
return sumX / n;
}
double standardDeviation() {
return Math.sqrt((sumXX - sumX * sumX / n) / (n - 1));
}
public int getN() {
return n;
}
public double getSumX() {
return sumX;
}
public double getSumXX() {
return sumXX;
}
public double getSumXY() {
return sumXY;
}
public double getSumY() {
return sumY;
}
public double getSumYY() {
return sumYY;
}
public double getBeta() {
return (sumXY/n - sumX * sumY/(n*n))
/ ((sumXX / n - (sumX * sumX / (n * n))));
}
public double getAlpha() {
return (sumY - getBeta() * sumX) / n;
}
public double getCorrelation() {
return (n * sumXY - sumX * sumY)
/ (sqrt(n * sumXX - sumX * sumX) * sqrt(n * sumYY - sumY * sumY));
}
@Override
public String toString() {
if(n<2)
return "Empty stats";
return "N=" + n + "\nb:"+(float)getBeta()+"\na:"+(float)getAlpha()+"\nC:"+(float)getCorrelation();
}
}
The module correctness was tested with a JUnit test case:
Code: Select all
package com.kvols.stat;
import org.junit.Test;
import static org.junit.Assert.*;
public class StatTest {
@Test
public void testAddMethod() {
Stat stat = new Stat();
stat.add(1, 2);
stat.add(2, 3);
stat.add(3, 4);
assertEquals("N", 3, stat.getN());
assertEquals("sum x", 6, stat.getSumX(), 1e-12);
assertEquals("sum y", 9, stat.getSumY(), 1e-12);
assertEquals("sum x²", 14, stat.getSumXX(), 1e-12);
assertEquals("sum xy", 20, stat.getSumXY(), 1e-12);
assertEquals("sum y²", 29, stat.getSumYY(), 1e-12);
System.out.println(stat);
}
@Test
public void testCalculations() {
Stat stat= new Stat();
stat.add(5,3);
stat.add(8,2);
stat.add(6,9);
stat.add(5,8);
stat.add(3,4);
stat.add(9,2);
assertEquals("mean", 6, stat.getMean(), 1e-12);
assertEquals("std.dev", 2.19089023002, stat.standardDeviation(), 1e-11);
assertEquals("correlation", -.385704038913, stat.getCorrelation(), 1e-11);
assertEquals("beta", -.541666666667, stat.getBeta(), 1e-11);
assertEquals("alpha", 7.91666666667, stat.getAlpha(), 1e-11);
}
}
The numbers for the test case (alpha, beta, correlation) was taken from my trusted HP50G calculator, and it matches.
I then created a small program for the LEGO brick to take some samples:
Code: Select all
import lejos.nxt.ADSensorPort;
import lejos.nxt.Battery;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;
import com.kvols.stat.Stat;
public class GVSampler {
public static void main(String[] args) throws InterruptedException {
Stat stat= new Stat();
ADSensorPort gyroPort= SensorPort.S1;
gyroPort.setType(SensorPort.POWER_9V);
for(;;) {
LCD.clear();
LCD.drawString(stat.toString(), 0, 0);
int key= Button.readButtons();
while(key == 0) {
LCD.drawString("Gyro:"+gyroPort.readValue(), 0, 6);
LCD.drawString("Batt:"+Battery.getVoltageMilliVolt(), 0, 7);
Thread.sleep(50);
key= Button.readButtons();
}
if(key==Button.ID_ENTER) {
LCD.clear();
LCD.drawString("Sampling...", 0, 3);
for(int i=0; i<100; i++) {
double batt= Battery.getVoltage();
double gyro= gyroPort.readValue();
batt= (Battery.getVoltage()+batt)/2;
stat.add(batt, gyro);
Thread.sleep(10);
}
}
if(key==Button.ID_ESCAPE) break;
}
}
}
The brick was then attached to a DC power supply, and 20 iterations of 100 samples was taken from approx. 5.5V to 9.5V.
The result is quite similar to the results from Aswin:
- Battery/Gyro offset correlation
- BattGyroCorrelation.jpg (52.12 KiB) Viewed 10811 times
For my sensor at this temperature, the approximate sensor offset would be:
Code: Select all
offset= 597.2054f + Battery.getVoltage() * 1.111321f
The correlation of 0.733 does indicate that there are other factors at stake here, eg. sensor noise, battery sensor noise. But still: It's probably a much better way of eliminating at least
some of the sensor noise and get a bit more predictive results!
I'll keep you posted when I have a new version with the battery voltage taken into account!
Povl
Doesn't the Kalman filter normally use matrices and vector arithmetic to do the calculations?)
