KalmanTest.h

Go to the documentation of this file.

// -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; coding: iso-8859-1; -*-

/*
ACT-CV - Machine Vision for Cognitive Modeling
Copyright (c) 2008 Marc Halbruegge

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/


#ifndef KALMANTEST_H_
#define KALMANTEST_H_

#include <iostream>
#include <general/OpenCVKalman.h>
#include "TestCase.h"

namespace tests {
    
class KalmanTest : public TestCase {
public:    
    const char* GetName() const {
        return "KalmanTest";
    }

    void Run() {
        
        KalmanFilter<1> kalman;

        for (int runs=0;runs < 100; runs++) {
            float x = (rand() % 1001) - 500;
            //std::cout << x << "\t";
            
            kalman.InitRandom(x, 1000.0, 1e-0, 1e-2);
            float state[2];
            float var[2];

            kalman.GetCorrectedState(state);
            kalman.GetVariance(var);

            AssertEqual(state[0], x, __FILE__, __LINE__);
            AssertEqual(state[1], 0, __FILE__, __LINE__);
            AssertEqual(var[0], 1000, __FILE__, __LINE__);
            AssertEqual(var[1], 1000, __FILE__, __LINE__);

            float trueVel = (rand() % 21) - 10;
            //std::cout << trueVel << "\t";
                        
            for (int i=1;i<100;i++) {
                state[0]=trueVel*i+x;
                state[1]=0;
                
                // test cvKalmanCorrect
                kalman.AddMeasure(state);
                kalman.GetCorrectedState(state);
                kalman.GetVariance(var);
                
                float zPos = (state[0]-(x+trueVel*i)) / sqrt(var[0]);
                float zVel = (state[1]-trueVel) / sqrt(var[1]);
                //std::cout << zPos << " " << zVel << std::endl;
                
                if (i>0) {
                    AssertBelow(fabs(zPos), 1, __FILE__, __LINE__);
                    AssertBelow(fabs(zVel), 1, __FILE__, __LINE__);
                }

                // test cvKalmanPredict
                kalman.Predict(state);
                kalman.GetVariance(var);
                  
                zPos = (state[0]-(x+trueVel*(i+1))) / sqrt(var[0]);
                zVel = (state[1]-trueVel) / sqrt(var[1]);
                //std::cout << zPos << " " << zVel << std::endl;

                if (i>1) {
                    AssertBelow(fabs(zPos), 1, __FILE__, __LINE__);
                    AssertBelow(fabs(zVel), 1, __FILE__, __LINE__);
                }
                
#if 0
                std::cout
                    << x << " " 
                    << state[0] << " "
                    << state[1] << " " 
                    << std::endl;
                std::cout
                    << var[0] << " "
                    << var[1] << " " 
                    << std::endl;
#endif                    
            }              
        }
    }
};

}; // end of namespace tests

#endif /*KALMANTEST_H_*/