00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <iostream>
00020 #include <string>
00021 #include "uvsim/utilities/utilities.h"
00022 #include <uvsim/navigation/KalmanFilter.h>
00023 #include <uvsim/dynamics/LinearModel.h>
00024 #include <iostream>
00025
00026 using namespace uvsim;
00027 using namespace boost::numeric::ublas;
00028
00029 class SimpleModel : public LinearModel
00030 {
00031 public:
00032 SimpleModel(vector<double> *x, vector<double> *u) :
00033 LinearModel(x,u)
00034 {
00035 }
00036
00037 ~SimpleModel()
00038 {
00039 }
00040
00041 void initialize()
00042 {
00043 size(3,1,1);
00044 update();
00045
00046
00047 Ed = identity_matrix<double>(n);
00048
00049
00050 L = identity_matrix<double>(m);
00051
00052 }
00053
00054 void update()
00055 {
00056
00057 Ad(0,0) = 1.1269;
00058 Ad(0,1) = -0.4940;
00059 Ad(0,2) = 0.1129;
00060 Ad(1,0) = 1;
00061 Ad(2,1) = 1;
00062
00063
00064 Bd(0,0) = -0.3832;
00065 Bd(1,0) = 0.5919;
00066 Bd(2,0) = 0.5191;
00067
00068
00069 C(0,0) = 1;
00070 }
00071
00072 };
00073
00074 int main()
00075 {
00076 int n = 3;
00077 int m = 1;
00078 int p = 1;
00079
00080 int kfFreq = 10;
00081 int nFinal = 1000;
00082
00083
00084 vector<double> x(n), w(n), v(m), y(m), u(p);
00085 matrix<double> P(n,n);
00086
00087
00088 x = zero_vector<double>(n);
00089 P = identity_matrix<double>(n)*2;
00090 y = zero_vector<double>(m);
00091 u = zero_vector<double>(p);
00092
00093
00094 SimpleModel md(&x, &u);
00095 md.initialize();
00096 md.print();
00097 KalmanFilter kf(&md, &y, &P);
00098 kf.print();
00099
00100
00101 for (int i=0; i<nFinal; i++)
00102 {
00103 u(0) = 1;
00104 y(0) = 1;
00105 std::cout << "y: " << y << std::endl;
00106 kf.update();
00107 kf.print();
00108 }
00109 return(0);
00110 }
00111
00112