Skip to content

Instantly share code, notes, and snippets.

Created December 7, 2015 06:04
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
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
// function to print out floats in HEX
void serialFloatPrint(float f) {
byte * b = (byte *) &f;
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;
Copy link

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

Copy link

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.

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.

Copy link

thank you brother for the great worked perfectly for me 👍

Copy link

SadokG commented Aug 29, 2022

thank you so much my brother for this update

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment