00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072 #include "plane.h"
00073 #include <cmath>
00074 #include <iostream>
00075
00076 using namespace std;
00077
00078 cPlaneEKF::cPlaneEKF()
00079 {
00080 setDim(4, 1, 2, 2, 2);
00081 Period = 0.2;
00082 Gravity = 9.8;
00083 Bfriction = 0.35;
00084 Portance = 3.92;
00085 Mass = 1000;
00086 }
00087
00088 void cPlaneEKF::makeBaseA()
00089 {
00090 A(1,1) = 1.0;
00091
00092 A(1,3) = 0.0;
00093 A(1,4) = 0.0;
00094
00095 A(2,1) = 0.0;
00096
00097 A(2,3) = 0.0;
00098 A(2,4) = 0.0;
00099
00100 A(3,1) = 0.0;
00101
00102 A(3,3) = 1.0;
00103 A(3,4) = Period;
00104
00105 A(4,1) = 0.0;
00106
00107 A(4,3) = 0.0;
00108 A(4,4) = 1.0;
00109 }
00110
00111 void cPlaneEKF::makeA()
00112 {
00113
00114 A(1,2) = Period - Period*Period*Bfriction/Mass*x(2);
00115
00116
00117
00118
00119 A(2,2) = 1 - 2*Period*Bfriction/Mass*x(2);
00120
00121
00122
00123
00124 A(3,2) = Period*Period*Portance/Mass*x(2);
00125
00126
00127
00128
00129 A(4,2) = 2*Period*Portance/Mass*x(2);
00130
00131
00132 }
00133
00134 void cPlaneEKF::makeBaseW()
00135 {
00136 W(1,1) = 0.0;
00137 W(1,2) = 0.0;
00138 W(2,1) = 1.0;
00139 W(2,2) = 0.0;
00140 W(3,1) = 0.0;
00141 W(3,2) = 0.0;
00142 W(4,1) = 0.0;
00143 W(4,2) = 1.0;
00144 }
00145
00146 void cPlaneEKF::makeBaseQ()
00147 {
00148 Q(1,1) = 0.01*0.01;
00149 Q(1,2) = 0.01*0.01/10.0;
00150 Q(2,1) = 0.01*0.01/10.0;
00151 Q(2,2) = 0.01*0.01;
00152 }
00153
00154 void cPlaneEKF::makeBaseH()
00155 {
00156
00157 H(1,2) = 0.0;
00158
00159 H(1,4) = 0.0;
00160
00161
00162 H(2,2) = 0.0;
00163
00164 H(2,4) = 0.0;
00165 }
00166
00167 void cPlaneEKF::makeH()
00168 {
00169 H(1,1) = -x(3)/(x(1)*x(1)+x(3)*x(3));
00170
00171 H(1,3) = x(1)/(x(1)*x(1)+x(3)*x(3));
00172
00173
00174 H(2,1) = x(1)/sqrt(x(1)*x(1)+x(3)*x(3));
00175
00176 H(2,3) = x(3)/sqrt(x(1)*x(1)+x(3)*x(3));
00177
00178 }
00179
00180 void cPlaneEKF::makeBaseV()
00181 {
00182 V(1,1) = 1.0;
00183 V(2,2) = 1.0;
00184 }
00185
00186 void cPlaneEKF::makeBaseR()
00187 {
00188 R(1,1) = 0.01*0.01;
00189 R(2,2) = 50*50;
00190 }
00191
00192 void cPlaneEKF::makeProcess()
00193 {
00194 Vector x_(x.size());
00195 x_(1) = x(1) + x(2)*Period + (Period*Period)/2*(u(1)/Mass - Bfriction/Mass*x(2)*x(2));
00196 x_(2) = x(2) + (u(1)/Mass - Bfriction/Mass*x(2)*x(2))*Period;
00197 x_(3) = x(3) + x(4)*Period + (Period*Period)/2*(Portance/Mass*x(2)*x(2)-Gravity);
00198 x_(4) = x(4) + (Portance/Mass*x(2)*x(2)-Gravity)*Period;
00199 x.swap(x_);
00200 }
00201
00202 void cPlaneEKF::makeMeasure()
00203 {
00204 z(1)=atan2(x(3), x(1));
00205 z(2)=sqrt(x(1)*x(1)+x(3)*x(3));
00206 }
00207