Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
// Filter temperature sensor readings using the Kalman process
const int sensorPin = A0; // named constant for the pin the sensor is connected to
// kalman variables
float varVolt = 1.12184278324081E-05; // variance determined using excel and reading samples of raw sensor data
float varProcess = 1e-8;
float Pc = 0.0;
float G = 0.0;
float P = 1.0;
float Xp = 0.0;
float Zp = 0.0;
float Xe = 0.0;
void setup() {
// open a serial connection to display values
Serial.begin(9600);
}
void loop() {
int sensorVal = analogRead(sensorPin); // read the value on AnalogIn pin 0 and store it in a variable
float voltage = sensorVal * 5.0 / 1023.0; // convert the ADC reading to voltage
// kalman process
Pc = P + varProcess;
G = Pc/(Pc + varVolt); // kalman gain
P = (1-G)*Pc;
Xp = Xe;
Zp = Xp;
Xe = G*(voltage-Zp)+Xp; // the kalman estimate of the sensor voltage
serialFloatPrint(voltage);
serialFloatPrint(Xe);
Serial.println();
delay(20);
}
// function to print out floats in HEX
void serialFloatPrint(float f) {
byte * b = (byte *) &f;
Serial.print("#");
for(int i=0; i<4; i++) {
byte b1 = (b[i] >> 4) & 0x0f;
byte b2 = (b[i] & 0x0f);
char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
Serial.print(c1);
Serial.print(c2);
}
}
@Mdevgaonkar

This comment has been minimized.

Copy link

Mdevgaonkar commented Mar 15, 2016

can u please explain on how to select variance and also can this code be used with HMC5883l magnetometer just for reading direction in degrees

@sassy-ankit

This comment has been minimized.

Copy link

sassy-ankit commented Mar 16, 2016

@Mdevgaonkar
Usually sensor noise covariance (R) = Rate Noise Density, which is given in Datasheet. But if you need exact value for the hardware you are using, then simply take around 50000 sampled values from the sensor which it is not rotating or moving and find out the ALLAN VARIANCE. In my experience, normal variance (var command in matlab) doesn't give good values compared to ALLAN VARIANCE.

@benjamin2044

This comment has been minimized.

Copy link

benjamin2044 commented Feb 8, 2018

Hi, I have watched your youtube video about Kalman filter. I have one question. Why didn't you update "P" variable, or "Pc" variable in the loop? If you don't update then your Kalman gain "G" is going to be same for every observation since G = Pc/(Pc + varVolt) and P = (1-G)*Pc.
But Kalman gain is supposed to change per every observation.

@abdulsamad381

This comment has been minimized.

Copy link

abdulsamad381 commented Jan 21, 2019

thank you brother for the great help.it worked perfectly for me 👍

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.