Chun Feng Huang / SLIP_ACCELERATION

The library for calculating and processing the slip acceleration

SLIP_ACCELERATION.cpp@4:7b8e13c54dfb, 2017-02-07 (annotated)

Committer:
benson516
Date:
Tue Feb 07 09:54:15 2017 +0000
Revision:
4:7b8e13c54dfb
Parent:
3:265a69f4c360 
test succeed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:4d4bddd95444 1 #include "SLIP_ACCELERATION.h"
benson516 0:4d4bddd95444 2
benson516 3:265a69f4c360 3 SLIP_ACCELERATION_2WHEEL::SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, size_t wheelDelay_ms_in, float samplingTime):
benson516 0:4d4bddd95444 4 Ts(samplingTime),
benson516 0:4d4bddd95444 5 onBoardDelay_ms(onBoardDelay_ms_in),
benson516 3:265a69f4c360 6 wheelDelay_ms(wheelDelay_ms_in),
benson516 0:4d4bddd95444 7 yawAcce_cal(samplingTime),
benson516 4:7b8e13c54dfb 8 lpf_dVs(2, samplingTime, 300.0, 1), // 200 Hz
benson516 3:265a69f4c360 9 // hpf_dVs(2,samplingTime, 0.01, 4), // 0.1 Hz, 4th order critical-damped HPF
benson516 4:7b8e13c54dfb 10 hpf_dVs(2,samplingTime, 0.01, 1), // 0.1 Hz, 4th order critical-damped HPF
benson516 4:7b8e13c54dfb 11 lpf_dVs_biasedEst(2,samplingTime, 0.5, 2), // 0.5 Hz
benson516 3:265a69f4c360 12 OnboardSignal_FIFO(), // Empty
benson516 3:265a69f4c360 13 wheelSignal_FIFO() // Empty
benson516 0:4d4bddd95444 14 {
benson516 0:4d4bddd95444 15 //
benson516 0:4d4bddd95444 16 n = 2; // This class is made specifically for differential-drive vehicle
benson516 0:4d4bddd95444 17 //
benson516 0:4d4bddd95444 18 zeros_2.assign(2,0.0);
benson516 3:265a69f4c360 19 // Not initialized
benson516 3:265a69f4c360 20 Flag_Init = false;
benson516 3:265a69f4c360 21 init_count = 0;
benson516 0:4d4bddd95444 22
benson516 0:4d4bddd95444 23 // Parameters
benson516 0:4d4bddd95444 24 btotal = 0.194; // 0.2; // m
benson516 0:4d4bddd95444 25 br = 0.103; // btotal/2.0; // m
benson516 0:4d4bddd95444 26 bl = btotal - br; // m
benson516 0:4d4bddd95444 27 r = 0.139/2.0; // m
benson516 0:4d4bddd95444 28
benson516 0:4d4bddd95444 29 // Inputs
benson516 0:4d4bddd95444 30 acce_vehicleCenter = 0.0;
benson516 0:4d4bddd95444 31 // wheelRotationalAcce = zeros_2;
benson516 0:4d4bddd95444 32 acce_wheelSpeed = zeros_2;
benson516 0:4d4bddd95444 33 yawRate = 0.0;
benson516 0:4d4bddd95444 34
benson516 0:4d4bddd95444 35 // States
benson516 0:4d4bddd95444 36 yawAcce = 0.0;
benson516 0:4d4bddd95444 37 acce_wheelCenter = zeros_2;
benson516 0:4d4bddd95444 38 acce_wheelCenter_delay = zeros_2;
benson516 3:265a69f4c360 39 acce_wheelSpeed_delay = zeros_2;
benson516 0:4d4bddd95444 40
benson516 0:4d4bddd95444 41 // Results
benson516 0:4d4bddd95444 42 dVs = zeros_2;
benson516 3:265a69f4c360 43 dVs_filtered = zeros_2;
benson516 3:265a69f4c360 44 //
benson516 3:265a69f4c360 45 dVs_unbiased = zeros_2;
benson516 3:265a69f4c360 46 dVs_bias = zeros_2;
benson516 0:4d4bddd95444 47
benson516 0:4d4bddd95444 48
benson516 0:4d4bddd95444 49 // Initialize the queue
benson516 0:4d4bddd95444 50 for (size_t i = 0; i < onBoardDelay_ms; ++i){
benson516 0:4d4bddd95444 51 OnboardSignal_FIFO.push(zeros_2);
benson516 0:4d4bddd95444 52 }
benson516 3:265a69f4c360 53 // Initialize the queue
benson516 3:265a69f4c360 54 for (size_t i = 0; i < onBoardDelay_ms; ++i){
benson516 3:265a69f4c360 55 wheelSignal_FIFO.push(zeros_2);
benson516 3:265a69f4c360 56 }
benson516 0:4d4bddd95444 57 }
benson516 0:4d4bddd95444 58 vector<float> SLIP_ACCELERATION_2WHEEL::iterateOnce(void){ // Calculate dVs
benson516 0:4d4bddd95444 59
benson516 3:265a69f4c360 60 // Initialization
benson516 3:265a69f4c360 61 if (!Flag_Init){
benson516 3:265a69f4c360 62 if (init_count <= (max(wheelDelay_ms, onBoardDelay_ms)+1) ){
benson516 3:265a69f4c360 63 dVs_filtered = zeros_2;
benson516 3:265a69f4c360 64 init_count++;
benson516 3:265a69f4c360 65 return dVs_filtered;
benson516 3:265a69f4c360 66 }
benson516 3:265a69f4c360 67 dVs_bias = zeros_2;
benson516 3:265a69f4c360 68 dVs_unbiased = dVs;
benson516 3:265a69f4c360 69 dVs_filtered = zeros_2;
benson516 3:265a69f4c360 70 //
benson516 3:265a69f4c360 71 resetFilter();
benson516 3:265a69f4c360 72 Flag_Init = true;
benson516 3:265a69f4c360 73 return dVs_filtered; // output = 0.0
benson516 3:265a69f4c360 74 }
benson516 0:4d4bddd95444 75
benson516 0:4d4bddd95444 76 // Claculate the latest signal
benson516 0:4d4bddd95444 77 acce_wheelCenter_cal();
benson516 0:4d4bddd95444 78
benson516 0:4d4bddd95444 79 // Pushing queue (On-board signals need to be delayed)
benson516 3:265a69f4c360 80 // queueOp(acce_wheelCenter_delay,acce_wheelCenter);
benson516 3:265a69f4c360 81 queueOp(acce_wheelCenter_delay, acce_wheelCenter, OnboardSignal_FIFO, onBoardDelay_ms);
benson516 3:265a69f4c360 82 queueOp(acce_wheelSpeed_delay, acce_wheelSpeed, wheelSignal_FIFO, wheelDelay_ms);
benson516 0:4d4bddd95444 83
benson516 0:4d4bddd95444 84 // Calculate the dVs
benson516 3:265a69f4c360 85 /*
benson516 0:4d4bddd95444 86 for (size_t i = 0; i < n; ++i){
benson516 0:4d4bddd95444 87 // acce_wheelSpeed[i] = r*wheelRotationalAcce[i];
benson516 3:265a69f4c360 88 // dVs[i] = acce_wheelSpeed[i] - acce_wheelCenter_delay[i];
benson516 3:265a69f4c360 89 dVs[i] = acce_wheelSpeed_delay[i] - acce_wheelCenter_delay[i];
benson516 0:4d4bddd95444 90 }
benson516 3:265a69f4c360 91 */
benson516 3:265a69f4c360 92
benson516 3:265a69f4c360 93 // dVs = acce_wheelSpeed_delay - acce_wheelCenter_delay;
benson516 3:265a69f4c360 94 dVs = Get_VectorPlus(acce_wheelSpeed_delay, acce_wheelCenter_delay, true); // minus
benson516 3:265a69f4c360 95
benson516 0:4d4bddd95444 96
benson516 0:4d4bddd95444 97 // Filtering the dVs
benson516 0:4d4bddd95444 98 // dVs_filtered = dVs; // Nothing to do
benson516 0:4d4bddd95444 99 // dVs_filtered = lpf_dVs.filter(dVs); // Low-pass: 0.0 Hz ~ 200 Hz
benson516 3:265a69f4c360 100 //
benson516 2:e275f3919042 101 // dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs)); // Band-pass: 0.015 Hz ~ 200 Hz
benson516 3:265a69f4c360 102
benson516 4:7b8e13c54dfb 103
benson516 4:7b8e13c54dfb 104
benson516 3:265a69f4c360 105 // dVs_unbiased = dVs - dVs_bias
benson516 3:265a69f4c360 106 dVs_unbiased = Get_VectorPlus(dVs, dVs_bias, true); // minus
benson516 4:7b8e13c54dfb 107 dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs_unbiased)); // Band-pass: 0.015 Hz ~ 200 Hz
benson516 3:265a69f4c360 108
benson516 4:7b8e13c54dfb 109
benson516 4:7b8e13c54dfb 110 /*
benson516 4:7b8e13c54dfb 111 // dVs_unbiased = dVs - dVs_bias
benson516 4:7b8e13c54dfb 112 dVs_unbiased = lpf_dVs.filter( Get_VectorPlus(dVs, dVs_bias, true) ); // minus
benson516 4:7b8e13c54dfb 113 dVs_filtered = hpf_dVs.filter(dVs_unbiased); // Band-pass: 0.015 Hz ~ 200 Hz
benson516 4:7b8e13c54dfb 114 */
benson516 4:7b8e13c54dfb 115
benson516 4:7b8e13c54dfb 116 lpf_dVs_biasedEst.filter(dVs); // Get the estimation of the bias in dVs
benson516 4:7b8e13c54dfb 117
benson516 0:4d4bddd95444 118
benson516 0:4d4bddd95444 119 //
benson516 0:4d4bddd95444 120 // return dVs;
benson516 0:4d4bddd95444 121 return dVs_filtered;
benson516 0:4d4bddd95444 122 }
benson516 3:265a69f4c360 123 void SLIP_ACCELERATION_2WHEEL::resetFilter(void){ // Rest all filters, includeing LPF and HPF
benson516 3:265a69f4c360 124 // Bias calculation
benson516 3:265a69f4c360 125 // dVs_bias += dVs_unbiased - dVs_filtered
benson516 4:7b8e13c54dfb 126 // Get_VectorIncrement(dVs_bias, Get_VectorPlus(dVs_unbiased, dVs_filtered, true), false); // +=
benson516 4:7b8e13c54dfb 127
benson516 4:7b8e13c54dfb 128 // Set the bias as the estimation of bias
benson516 4:7b8e13c54dfb 129 dVs_bias = lpf_dVs_biasedEst.output;
benson516 4:7b8e13c54dfb 130 dVs_unbiased = zeros_2;
benson516 4:7b8e13c54dfb 131
benson516 3:265a69f4c360 132 //
benson516 3:265a69f4c360 133 yawAcce_cal.reset(yawRate);
benson516 3:265a69f4c360 134 lpf_dVs.reset(zeros_2);
benson516 3:265a69f4c360 135 hpf_dVs.reset(zeros_2);
benson516 3:265a69f4c360 136 dVs_filtered = zeros_2;
benson516 3:265a69f4c360 137 }
benson516 0:4d4bddd95444 138 //
benson516 0:4d4bddd95444 139 void SLIP_ACCELERATION_2WHEEL::acce_wheelCenter_cal(void){ // Calculate the linear acceleration at each wheel center
benson516 0:4d4bddd95444 140 // Calculate the yawAcce
benson516 0:4d4bddd95444 141 yawAcce = yawAcce_cal.filter(yawRate);
benson516 0:4d4bddd95444 142
benson516 0:4d4bddd95444 143 // Calculate the linear acceleration at each wheel center
benson516 0:4d4bddd95444 144 // acce_wheelCenter[0] = acce_vehicleCenter; // Right
benson516 0:4d4bddd95444 145 // acce_wheelCenter[1] = acce_vehicleCenter; // Left
benson516 0:4d4bddd95444 146 //
benson516 0:4d4bddd95444 147 acce_wheelCenter[0] = acce_vehicleCenter + yawAcce*br; // Right
benson516 3:265a69f4c360 148 acce_wheelCenter[1] = acce_vehicleCenter - yawAcce*bl; // Left
benson516 0:4d4bddd95444 149 }
benson516 0:4d4bddd95444 150 //
benson516 0:4d4bddd95444 151 void SLIP_ACCELERATION_2WHEEL::queueOp(vector<float> &signal_delayed, const vector<float> &signal_in){ // The operation of the queue
benson516 0:4d4bddd95444 152
benson516 0:4d4bddd95444 153 // Push into queue first to prevent the "empty" error when onBoardDelay_ms is zero
benson516 0:4d4bddd95444 154 OnboardSignal_FIFO.push(signal_in);
benson516 0:4d4bddd95444 155
benson516 0:4d4bddd95444 156 //----------------------------------------------------------//
benson516 0:4d4bddd95444 157
benson516 0:4d4bddd95444 158 // Get the delayed signal: signal_delayed
benson516 0:4d4bddd95444 159 signal_delayed = OnboardSignal_FIFO.front();
benson516 0:4d4bddd95444 160
benson516 0:4d4bddd95444 161 // Control the buffer size
benson516 0:4d4bddd95444 162 // Currently, the size of the queue is expected to be (onBoardDelay_ms + 1)
benson516 0:4d4bddd95444 163 if (OnboardSignal_FIFO.size() <= onBoardDelay_ms){
benson516 0:4d4bddd95444 164 // nothing to do
benson516 0:4d4bddd95444 165 }else{
benson516 0:4d4bddd95444 166 //
benson516 0:4d4bddd95444 167 while(OnboardSignal_FIFO.size() > onBoardDelay_ms){
benson516 0:4d4bddd95444 168 OnboardSignal_FIFO.pop();
benson516 0:4d4bddd95444 169 }
benson516 0:4d4bddd95444 170 // Now: OnboardSignal_FIFO.size() = onBoardDelay_ms
benson516 0:4d4bddd95444 171 }
benson516 0:4d4bddd95444 172 }
benson516 3:265a69f4c360 173 void SLIP_ACCELERATION_2WHEEL::queueOp(vector<float> &signal_delayed, const vector<float> &signal_in, queue<vector<float> > &queue_in, size_t queue_length){ // The operation of the queue
benson516 3:265a69f4c360 174
benson516 3:265a69f4c360 175 // Push into queue first to prevent the "empty" error when queue_length is zero
benson516 3:265a69f4c360 176 queue_in.push(signal_in);
benson516 3:265a69f4c360 177
benson516 3:265a69f4c360 178 //----------------------------------------------------------//
benson516 3:265a69f4c360 179
benson516 3:265a69f4c360 180 // Get the delayed signal: signal_delayed
benson516 3:265a69f4c360 181 signal_delayed = queue_in.front();
benson516 3:265a69f4c360 182
benson516 3:265a69f4c360 183 // Control the buffer size
benson516 3:265a69f4c360 184 // Currently, the size of the queue is expected to be (queue_length + 1)
benson516 3:265a69f4c360 185 if (queue_in.size() <= queue_length){
benson516 3:265a69f4c360 186 // nothing to do
benson516 3:265a69f4c360 187 }else{
benson516 3:265a69f4c360 188 //
benson516 3:265a69f4c360 189 while(queue_in.size() > queue_length){
benson516 3:265a69f4c360 190 queue_in.pop();
benson516 3:265a69f4c360 191 }
benson516 3:265a69f4c360 192 // Now: queue_in.size() = queue_length
benson516 3:265a69f4c360 193 }
benson516 3:265a69f4c360 194 }
benson516 3:265a69f4c360 195 // Utilities
benson516 3:265a69f4c360 196 void SLIP_ACCELERATION_2WHEEL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 3:265a69f4c360 197 static vector<float>::iterator it_out;
benson516 3:265a69f4c360 198 static vector<const float>::iterator it_m_row;
benson516 3:265a69f4c360 199 static vector<const float>::iterator it_v;
benson516 3:265a69f4c360 200 //
benson516 3:265a69f4c360 201 it_out = v_out.begin();
benson516 3:265a69f4c360 202 for (size_t i = 0; i < m_left.size(); ++i){
benson516 3:265a69f4c360 203 *it_out = 0.0;
benson516 3:265a69f4c360 204 it_m_row = m_left[i].begin();
benson516 3:265a69f4c360 205 it_v = v_right.begin();
benson516 3:265a69f4c360 206 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 3:265a69f4c360 207 // *it_out += m_left[i][j] * v_right[j];
benson516 3:265a69f4c360 208 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 3:265a69f4c360 209 (*it_out) += (*it_m_row) * (*it_v);
benson516 3:265a69f4c360 210 }else{
benson516 3:265a69f4c360 211 // (*it_out) += 0.0
benson516 3:265a69f4c360 212 }
benson516 3:265a69f4c360 213 // (*it_out) += (*it_m_row) * (*it_v);
benson516 3:265a69f4c360 214 //
benson516 3:265a69f4c360 215 it_m_row++;
benson516 3:265a69f4c360 216 it_v++;
benson516 3:265a69f4c360 217 }
benson516 3:265a69f4c360 218 it_out++;
benson516 3:265a69f4c360 219 }
benson516 3:265a69f4c360 220 }
benson516 3:265a69f4c360 221 vector<float> SLIP_ACCELERATION_2WHEEL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
benson516 3:265a69f4c360 222 static vector<float> v_out;
benson516 3:265a69f4c360 223 // Size check
benson516 3:265a69f4c360 224 if (v_out.size() != m_left.size()){
benson516 3:265a69f4c360 225 v_out.resize(m_left.size());
benson516 3:265a69f4c360 226 }
benson516 3:265a69f4c360 227 // Iterators
benson516 3:265a69f4c360 228 static vector<float>::iterator it_out;
benson516 3:265a69f4c360 229 static vector<const float>::iterator it_m_row;
benson516 3:265a69f4c360 230 static vector<const float>::iterator it_v;
benson516 3:265a69f4c360 231 //
benson516 3:265a69f4c360 232 it_out = v_out.begin();
benson516 3:265a69f4c360 233 for (size_t i = 0; i < m_left.size(); ++i){
benson516 3:265a69f4c360 234 *it_out = 0.0;
benson516 3:265a69f4c360 235 it_m_row = m_left[i].begin();
benson516 3:265a69f4c360 236 it_v = v_right.begin();
benson516 3:265a69f4c360 237 for (size_t j = 0; j < m_left[i].size(); ++j){
benson516 3:265a69f4c360 238 // *it_out += m_left[i][j] * v_right[j];
benson516 3:265a69f4c360 239 if (*it_m_row != 0.0 && *it_v != 0.0){
benson516 3:265a69f4c360 240 (*it_out) += (*it_m_row) * (*it_v);
benson516 3:265a69f4c360 241 }else{
benson516 3:265a69f4c360 242 // (*it_out) += 0.0
benson516 3:265a69f4c360 243 }
benson516 3:265a69f4c360 244 // (*it_out) += (*it_m_row) * (*it_v);
benson516 3:265a69f4c360 245 //
benson516 3:265a69f4c360 246 it_m_row++;
benson516 3:265a69f4c360 247 it_v++;
benson516 3:265a69f4c360 248 }
benson516 3:265a69f4c360 249 it_out++;
benson516 3:265a69f4c360 250 }
benson516 3:265a69f4c360 251 return v_out;
benson516 3:265a69f4c360 252 }
benson516 3:265a69f4c360 253 vector<float> SLIP_ACCELERATION_2WHEEL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
benson516 3:265a69f4c360 254 {
benson516 3:265a69f4c360 255 static vector<float> v_c;
benson516 3:265a69f4c360 256 // Size check
benson516 3:265a69f4c360 257 if (v_c.size() != v_a.size()){
benson516 3:265a69f4c360 258 v_c.resize(v_a.size());
benson516 3:265a69f4c360 259 }
benson516 3:265a69f4c360 260 //
benson516 3:265a69f4c360 261 for (size_t i = 0; i < v_a.size(); ++i){
benson516 3:265a69f4c360 262 if (is_minus){
benson516 3:265a69f4c360 263 v_c[i] = v_a[i] - v_b[i];
benson516 3:265a69f4c360 264 }else{
benson516 3:265a69f4c360 265 v_c[i] = v_a[i] + v_b[i];
benson516 3:265a69f4c360 266 }
benson516 3:265a69f4c360 267 }
benson516 3:265a69f4c360 268 return v_c;
benson516 3:265a69f4c360 269 }
benson516 3:265a69f4c360 270 vector<float> SLIP_ACCELERATION_2WHEEL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
benson516 3:265a69f4c360 271 {
benson516 3:265a69f4c360 272 static vector<float> v_c;
benson516 3:265a69f4c360 273 // Size check
benson516 3:265a69f4c360 274 if (v_c.size() != v_a.size()){
benson516 3:265a69f4c360 275 v_c.resize(v_a.size());
benson516 3:265a69f4c360 276 }
benson516 3:265a69f4c360 277 // for pure negative
benson516 3:265a69f4c360 278 if (scale == -1.0){
benson516 3:265a69f4c360 279 for (size_t i = 0; i < v_a.size(); ++i){
benson516 3:265a69f4c360 280 v_c[i] = -v_a[i];
benson516 3:265a69f4c360 281 }
benson516 3:265a69f4c360 282 return v_c;
benson516 3:265a69f4c360 283 }
benson516 3:265a69f4c360 284 // else
benson516 3:265a69f4c360 285 for (size_t i = 0; i < v_a.size(); ++i){
benson516 3:265a69f4c360 286 v_c[i] = scale*v_a[i];
benson516 3:265a69f4c360 287
benson516 3:265a69f4c360 288 }
benson516 3:265a69f4c360 289 return v_c;
benson516 3:265a69f4c360 290 }
benson516 3:265a69f4c360 291 // Increment
benson516 3:265a69f4c360 292 void SLIP_ACCELERATION_2WHEEL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
benson516 3:265a69f4c360 293 // Size check
benson516 3:265a69f4c360 294 if (v_a.size() != v_b.size()){
benson516 3:265a69f4c360 295 v_a.resize(v_b.size());
benson516 3:265a69f4c360 296 }
benson516 3:265a69f4c360 297 //
benson516 3:265a69f4c360 298 if (is_minus){ // -=
benson516 3:265a69f4c360 299 for (size_t i = 0; i < v_b.size(); ++i){
benson516 3:265a69f4c360 300 v_a[i] -= v_b[i];
benson516 3:265a69f4c360 301 }
benson516 3:265a69f4c360 302 }else{ // +=
benson516 3:265a69f4c360 303 for (size_t i = 0; i < v_b.size(); ++i){
benson516 3:265a69f4c360 304 v_a[i] += v_b[i];
benson516 3:265a69f4c360 305 }
benson516 3:265a69f4c360 306 }
benson516 3:265a69f4c360 307
benson516 3:265a69f4c360 308 }