# Using a Kalman filter on a single analog channel Things have been slow here at YardBot HQ recently. Not only do I work full time as a mobile software developer, but part time I work for a mechanical engineering company (same building and owners) that will be releasing a rather exciting computer product in the near future. Add it up together and it means that my robot aspirations are somewhat lower priority. Fortunately with winter tightening its grip here in Canada, I have a little extra time indoors to devote to learning more about ROS and its related systems. One of the things I wanted to learn more about is called the Kalman Filter.

This is a math process that comes up often when looking at IMU and navigation systems. As I understand it in simple terms, it revolves around the concept of filtering data by using previous measurements and merging that with the estimated variance between measurements to predict its current state. This can be done with 1 channel or, in the case of ROS’ robot_pose_ekf node, multiple sensors. The advantage of the extended Kalman filter in the IMU example is that you can combine sensors that have different levels of drift to retrieve a more accurate snapshot of the robot’s position. I haven’t done that yet, so I’m starting simple.

Currently I have 2 Arduinos sending back analog sensor data and controlling a DC motor and linear actuator. This analog data records things like temperature, humidity, current and voltage. Current and voltage both use the MEGA’s ADC pins, so they’re susceptible to noise. I decided to develop a reusable Kalman filter class for reducing or eliminating the noise on one of these single channels.

The result is seen in the photo above. The rocky blue line is the pure data coming in from the Arduino while the nice flat red line is the output of the Kalman filter node. Unfortunately using a voltage sensor for analog filtering doesn’t quite demonstrate the entire system, as the actual value doesn’t really move around, so I’ll be trying it with other sensor inputs later.

For demonstration, here are two files that were used: the single channel kalman filter class I wrote and the ROS node that takes advantage of it.

```#ifndef SingleChannelKalman_H
#define SingleChannelKalman_H

#include <math.h>

class SingleChannelKalman {
public:
SingleChannelKalman();
SingleChannelKalman(float _varP, float _varM, float _initial);
SingleChannelKalman(float _varP, float _varM);
float getVarP();
float getVarM();
float getValue();

private:
float p, varP, varM, k, value;
void setProperties(float _varP, float _varM, float _initial);
};

SingleChannelKalman::SingleChannelKalman() {

}

SingleChannelKalman::SingleChannelKalman(float _varP, float _varM, float _initial) {
setProperties(_varP, _varM, _initial);
}

SingleChannelKalman::SingleChannelKalman(float _varP, float _varM) {
setProperties(_varP, _varM, 0);
}

void SingleChannelKalman::setProperties(float _varP, float _varM, float _initial) {
varP = _varP;
varM = _varM;
value = _initial;
p = 1.0;
k = 1.0;
}

// compute the value
p += varP;
k = p / (p + varM);
value = k * input + (1 - k) * value;

// update the estimates
p = (1 - k) * p;

return value;
}

float SingleChannelKalman::getVarP() {
return varP;
}

float SingleChannelKalman::getVarM() {
return varM;
}

float SingleChannelKalman::getValue() {
return value;
}

#endif```

I pulled examples from multiple sites to get this class, and the core math came from this blog. The filter itself is used in a lot of different places so you should be able to find plenty of examples. That link includes a much deeper dive into the math, so give it a read. In this file, I start by defining the coming methods and variables. Essentially you can make an instance with the required filter properties, then add an input and get the new filtered value out.

```#include "ros/ros.h"
#include <math.h>
#include "single_channel_kalman.cpp"
#include <sensor_msgs/Temperature.h>
#include <std_msgs/Float32.h>

class KalmanVoltage {
public:
KalmanVoltage();
void updateParameters();

private:
void voltageCallback(const std_msgs::Float32::ConstPtr &msg);
SingleChannelKalman filter;
ros::Subscriber voltageSub;
ros::Publisher voltagePub;
ros::NodeHandle nh, _nh;

float varP, varM, initial;
};

KalmanVoltage::KalmanVoltage() {
_nh = ros::NodeHandle("~");
updateParameters();

filter = SingleChannelKalman(varP, varM, initial);
voltageSub = nh.subscribe("/arduino/voltage", 10, &KalmanVoltage::voltageCallback, this);
}

void KalmanVoltage::updateParameters() {
// update the parameters for filtering the data
if (!_nh.getParam("var_p", varP))
varP = pow(0.01, 2);

if (!_nh.getParam("var_m", varM))
varM = pow(0.5, 2);

if (!_nh.getParam("initial", initial))
initial = 12;
}

void KalmanVoltage::voltageCallback(const std_msgs::Float32::ConstPtr &msg) {

std_msgs::Float32 voltage_msg;
voltage_msg.data = value;
voltagePub.publish(voltage_msg);
}

int main(int argc, char** argv) {
ros::init(argc, argv, "kalman_voltage_node");
KalmanVoltage kalman_voltage_node;

ros::spin();

return 0;
}```

This is the file that makes it all work together. It’s configured like a typical ROS node: start the node and retrieve the included launch properties, then start up the subscribers and publishers. I create a subscriber for the unfiltered voltage topic then add a callback to generate the filtered response. In the callback you can also see where I add the input to the SingleChannelKalman instance configured earlier.

The end result is pretty solid as a first try, so I’ll be trying this setup with other analog inputs to see how it fairs.

Tagged with: , , , ,
###### One comment on “Using a Kalman filter on a single analog channel”
1. Donald Wright says:

I Wes, I did need all of your blogs but I find it interesting that you can run a Kalman filter. I did read a lot also I from my understanding, the Kalman filter tries to see ahead and tries to corrected from pass data the possible and estimated data before the PID does his job on correcting the errors. It’s like driving a car close to an invisible van so that you can predict the behavior of the van because you can see the red light,otherwise impossible to see if the van is not invisible! Oh, thanks for sharing! And hope to see more workinprogress!