diff --git a/math/realtime_stats.cpp b/math/realtime_stats.cpp
index f853b8274..d2b55a3cd 100644
--- a/math/realtime_stats.cpp
+++ b/math/realtime_stats.cpp
@@ -6,13 +6,16 @@
  * realtime. For example, devices reading biometrics data. The algorithm is
  * simple enough to be easily implemented in an embedded system.
  */
+#include <cassert>
 #include <cmath>
 #include <iostream>
 
 /**
  * continuous mean and variance computance using
- * first value as an approximation for the mean
- **/
+ * first value as an approximation for the mean.
+ * If the first number is much far form the mean, the algorithm becomes very
+ * inaccurate to compute variance and standard deviation.
+ */
 template <typename T>
 class stats_computer1 {
  public:
@@ -36,7 +39,9 @@ class stats_computer1 {
     /** return sample standard deviation computed till last sample */
     double std() const { return std::sqrt(this->variance()); }
 
-    /** short-hand operator to read new sample from input stream */
+    /** short-hand operator to read new sample from input stream
+     * \n e.g.: `std::cin >> stats1;`
+     */
     friend std::istream &operator>>(std::istream &input,
                                     stats_computer1 &stat) {
         T val;
@@ -53,8 +58,8 @@ class stats_computer1 {
 
 /**
  * continuous mean and variance computance using
- * Welford's algorithm
- **/
+ * Welford's algorithm  (very accurate)
+ */
 template <typename T>
 class stats_computer2 {
  public:
@@ -78,7 +83,9 @@ class stats_computer2 {
     /** return sample standard deviation computed till last sample */
     double std() const { return std::sqrt(this->variance()); }
 
-    /** short-hand operator to read new sample from input stream */
+    /** short-hand operator to read new sample from input stream
+     * \n e.g.: `std::cin >> stats1;`
+     */
     friend std::istream &operator>>(std::istream &input,
                                     stats_computer2 &stat) {
         T val;
@@ -92,8 +99,53 @@ class stats_computer2 {
     double mu = 0, var = 0, M = 0;
 };
 
+/** Test the algorithm implementation
+ * \param[in] test_data array of data to test the algorithms
+ */
+void test_function(const float *test_data, const int number_of_samples) {
+    float mean = 0.f, variance = 0.f;
+
+    stats_computer1<float> stats01;
+    stats_computer2<float> stats02;
+
+    for (int i = 0; i < number_of_samples; i++) {
+        stats01.new_val(test_data[i]);
+        stats02.new_val(test_data[i]);
+        mean += test_data[i];
+    }
+
+    mean /= number_of_samples;
+
+    for (int i = 0; i < number_of_samples; i++) {
+        float temp = test_data[i] - mean;
+        variance += temp * temp;
+    }
+    variance /= number_of_samples;
+
+    std::cout << "<<<<<<<< Test Function >>>>>>>>" << std::endl
+              << "Expected: Mean: " << mean << "\t Variance: " << variance
+              << std::endl;
+    std::cout << "\tMethod 1:"
+              << "\tMean: " << stats01.mean()
+              << "\t Variance: " << stats01.variance()
+              << "\t Std: " << stats01.std() << std::endl;
+    std::cout << "\tMethod 2:"
+              << "\tMean: " << stats02.mean()
+              << "\t Variance: " << stats02.variance()
+              << "\t Std: " << stats02.std() << std::endl;
+
+    assert(std::abs(stats01.mean() - mean) < 0.01);
+    assert(std::abs(stats02.mean() - mean) < 0.01);
+    assert(std::abs(stats02.variance() - variance) < 0.01);
+
+    std::cout << "(Tests passed)" << std::endl;
+}
+
 /** Main function */
 int main(int argc, char **argv) {
+    const float test_data1[] = {3, 4, 5, -1.4, -3.6, 1.9, 1.};
+    test_function(test_data1, sizeof(test_data1) / sizeof(test_data1[0]));
+
     std::cout
         << "Enter data. Any non-numeric data will terminate the data input."
         << std::endl;