int main (void)
{
void transposeMatrix (int nRows, int nCols,
int matrix1 [nRows][nCols],
int matrix2 [nRows][nCols]);
void displayMatrix (int nRows, int nCols, int matrix1[nRows][nCols]);
int matrix1[4][5] =
{
{ 1, 2, 3, 4, 5, },
{ 6, 7, 8, 9, 10, },
{ 11, 12, 13, 14, 15 },
{ 16, 17, 18, 19, 20 }
};
int matrix2[5][4] = {};
printf ("Original matrix:\n");
displayMatrix (4, 5, matrix1);
transposeMatrix (4, 5, matrix1);
printf ("Transposed matrix:\n");
displayMatrix (matrix1);
return 0;
}
int main (void){
void scalarMultiply(int nRows, int nCols, int matrix[nRows][nCols], int scalar);
void displayMatrix(int nRows, int nCols, int matrix[nRows][nCols]);
int sample_matrix[3][6] =
{
{7, 16, 55, 13, 12, 4},
{12, 10, 52, 0, 7, 77},
{-2, 1, 2, 4, 9, 63}
};
printf("Original matrix:\n");
displayMatrix(3, 6, sample_matrix);
scalarMultiply(3, 6, sample_matrix, 2);
printf("\nMultiplied by 2:\n");
displayMatrix(3, 6, sample_matrix);
scalarMultiply(3, 6, sample_matrix, -1);
printf("\nMultiplied by -1:\n");
displayMatrix(3, 6, sample_matrix);
return 0;
}
int main(int argc, char *argv[]) {
if(argc !=3 ) {
printf("Usage:\n<program_name> <rows> <columns>\n");
return -1;
}
int r=atoi(argv[1]);
int c=atoi(argv[2]);
int **matrix1 = createMatrix(r,c);
int **matrix2 = createMatrix(r,c);
printf("Matrix 1 :\n");
initMatrix(matrix1, r,c);
displayMatrix(matrix1, r,c);
printf("Matrix 2 :\n");
initMatrix(matrix2, r,c);
displayMatrix(matrix2, r,c);
int **sum_matrix3 = sumMatrix(matrix1, matrix2, r, c);
deleteMatrix(matrix1,r,c);
deleteMatrix(matrix2,r,c);
printf("\nSum Matrix 3 :\n");
displayMatrix(sum_matrix3, r, c);
deleteMatrix(sum_matrix3,r,c);
return 0;
}
int main(void)
{
void scalarMultiply(int matrix[3][5], int scalar);
void displayMatrix(int matrix[3][5]);
int sampleMatrix[3][5] =
{
{ 7, 16, 55, 13, 12},
{12, 10, 52, 0, 7},
{-2, 1, 2, 4, 9}
};
printf("Original matrix:\n");
displayMatrix(sampleMatrix);
scalarMultiply(sampleMatrix, 2);
printf("\nMultiplied by 2:\n");
displayMatrix(sampleMatrix);
scalarMultiply(sampleMatrix, -1);
printf("\nThen multiplied by -1:\n");
displayMatrix(sampleMatrix);
return 0;
}
/*
** Output transformations (for testing && diagnosis)
*/
void displayTransform(Transform t)
{
printf("\nTransform\n");
displayMatrix(t.transformation);
printf("Inverse Transform\n");
displayMatrix(t.inverseTransformation);
}
void Display(FunctionCall fc)
{
int index;
if (fc->name[0] == '*')
{
if (fc->name[1] == '\0' || fc->name[1] == 'V' || fc->name[1] == 'v')
{
printf("\n");
for (index=0; index<cur_var; index++)
{
printf("%24s == ", vars[index]->name);
displayE3(vars[index]->value, 0);
printf("\n");
}
}
if (fc->name[1] == '\0' || fc->name[1] == 'M' || fc->name[1] == 'm')
{
printf("\n");
for (index=0; index<cur_mat; index++)
{
printf("\tMatrix %s :\n", mats[index]->name);
displayMatrix(mats[index]->mat);
}
}
}
else
{
index = IndexMatrix(fc->name);
if (index==-1)
{
index = IndexVariable(fc->name);
if (index==-1) printf("\tSymbol %s Not Found\n", fc->name);
else
{
printf("\tVariable %s :", vars[index]->name);
displayE3(vars[index]->value, 6);
printf("\n");
}
}
else
{
printf("\tMatrix %s :\n", mats[index]->name);
displayMatrix(mats[index]->mat);
index = IndexVariable(fc->name);
if (index!=-1)
{
printf("\tVariable %s :", vars[index]->name);
displayE3(vars[index]->value, 6);
printf("\n");
}
}
}
}
void displayInt(int i) {
char str[5];
snprintf(str, 5, "%04d", i);
displayMatrix(str);
}
void lowBatCheck(void)
{
if(voltage < 700) //alert when battery Voltage lower than 7V
{
setLeftPwm(0);
setRightPwm(0);
while(1)
{
displayMatrix("Lbat");
//beep_on;
ALL_LED_OFF;
delay_ms(200);
clearScreen();
//beep_off;
ALL_LED_ON;
delay_ms(200);
}
}
else {
displayInt(voltage);
delay_ms(1000);
}
}
void Inversion(FunctionCall fc)
{
int i, j, index, exist = 0;
Matrix a, c;
char name[MAXSIZE_NAME] = {0};
index = IndexMatrix(fc->name);
if (index==-1)
{
index = cur_mat;
mats[index] = (StrMatObject*)malloc(sizeof(StrMatObject));
if (mats[index] == NULL)
{
printf("NewMatrix(): Could not allocate the new matrix\n");
return;
}
for (i = 0; i < MAXSIZE_NAME; i++)
{
mats[index]->name[i] = fc->name[i];
if (fc->name[i]=='\0') break;
}
}
else exist = 1;
j = 0;
// searching for matrix name
for (i = 0; i < MAXSIZE_NAME; i++)
{
name[j] = fc->args[i];
if (name[j] == '\0') break;
j++;
}
a = GetMatrix(name);
if (a==NULL)
{
printf("\tMatrix %s Not Found\n", name);
if (!exist) free(mats[index]);
return;
}
c = invert(a);
if (c==NULL)
{
printf("\tMatrix %s Not Invertible\n", name);
if (!exist) free(mats[index]);
return;
}
if (exist) deleteMatrix(mats[index]->mat);
mats[index]->mat = c;
// test: display the result
displayMatrix(mats[index]->mat);
if (!exist) cur_mat++;
}
void SamaelMainWindow::createWidgets()
{
QObject::connect(m_computationManager, SIGNAL(getClassNames(std::vector<std::string>&)), m_imageDataBase, SLOT(getClassNames(std::vector<std::string>&)));
QObject::connect(m_computationManager, SIGNAL(getTrainingImages(std::map<std::string, std::vector<SamaelImage*>>&)), m_imageDataBase, SLOT(getTrainingImages(std::map<std::string, std::vector<SamaelImage*>>&)));
QObject::connect(m_computationManager, SIGNAL(getClassifyImages(std::map<std::string, std::vector<SamaelImage*>>&)), m_imageDataBase, SLOT(getClassifyImages(std::map<std::string, std::vector<SamaelImage*>>&)));
// Central Widget
m_CentralWidget = new CentralWidget(this);
// Viewer Widget
this->setCentralWidget(m_CentralWidget);
QObject::connect(m_computationManager, SIGNAL(displayMatrix(cv::Mat)), m_CentralWidget->getDataViewerWidget(), SLOT(displayMatrix(cv::Mat)));
QObject::connect(m_computationManager, SIGNAL(setConfusionMatrixHeaderData(std::vector<std::string>&)), m_CentralWidget->getDataViewerWidget(), SLOT(setConfusionMatrixHeaderData(std::vector<std::string>&)));
// Terminal Widget
m_TerminalWidget->setParent(this);
this->addDockWidget(Qt::BottomDockWidgetArea, m_TerminalWidget);
// Tree Widget
m_TreeWidget = new TreeWidget(this);
this->addDockWidget(Qt::LeftDockWidgetArea, m_TreeWidget);
QObject::connect(m_TreeWidget, SIGNAL(addImageToDatabase(std::string, SamaelImage*)), m_imageDataBase, SLOT(addImage(std::string, SamaelImage*)));
QObject::connect(m_TreeWidget, SIGNAL(removeClassFromDatabase(std::string)), m_imageDataBase, SLOT(removeImages(std::string)));
// Toolbox Widget
m_ToolBox = new ToolBox(this);
this->addDockWidget(Qt::RightDockWidgetArea, m_ToolBox);
QObject::connect(m_ToolBox, SIGNAL(doClassification()), m_computationManager, SLOT(doClassification()));
}
int main (int argc, char *argv[]) {
if (argc != 2) {
printf("usage: ./tokenize tm_file\n");
exit(1);
} else if (argc == 2) {
FILE *fp;
fp = fopen ( argv[1], "r");
if ( fp == NULL ) {
// something went wrong
perror( argv[1] );
exit( 1 );
}
// build matrix
TransMatrix transMatrix = BuildMatrix(fp);
// print matrix
displayMatrix(transMatrix);
/* process stdin and print state transitions from start
to accepting state until end of file
*/
while(Scanner(transMatrix) != EOF);
//Scanner(transMatrix);
// close the stream
fclose(fp);
}
return 0;
}
void evaluateASTNodeExpression(ASTNode *node){
MatList *p;
Matrix result;
ASTNode *argNode[10];
Number figure;
switch (node->subType) {
case ExpSimpleAssign:
p = intoMatList(node->l->identifier,node->r->mat);
matrixList->mat = cloneMatrix(p->mat);
displayInternalInfoOfMatlist(p);
node->mat = cloneMatrix(node->r->mat);
break;
case ExpPartAssign: /* Still not written*/
p = checkMatList(node->l->identifier);
argNode[0] = getNthArgFromArgList(node->r,1);
argNode[1] = getNthArgFromArgList(node->r,2);
figure = readOneElementOfMatrix(node->a->mat,1,1);
matrixList->mat = createEyeMatrix(1);
changeOneElementOfMatrix(matrixList->mat,1,1,figure);
changeOneElementOfMatrix(p->mat,(int)readOneElementOfMatrix(argNode[0]->mat,1,1),(int)readOneElementOfMatrix(argNode[1]->mat,1,1),figure);
printf("%s(%d,%d) = \n",p->identifier,(int)readOneElementOfMatrix(argNode[0]->mat,1,1),(int)readOneElementOfMatrix(argNode[1]->mat,1,1));
printf("%f\n",figure);
break;
case ExpSimpleExp:
/*just clone the result*/
node->mat = cloneMatrix(node->l->mat);
matrixList->mat = cloneMatrix(node->mat);
displayMatrix(node->mat);
break;
default:
break;
}
return;
}
void displayMatrixScroll(char* str) {
int i;
for(i = 0; i < 3; i++) {
if(str[i] == '\0') {
displayMatrix(str);
return;
}
}
i = 0;
while(str[i+3] != '\0') {
displayMatrix(&str[i]);
delay_ms(1000);
i++;
}
}
void displayFloat(float f) {
//a string with the format xx.x
char str[5]; //need 5 because of null byte
snprintf(str, 5, "%.1f", f);
displayMatrix(str);
}
int main(int argc, char *argv[]) {
if(argc != 5) {
printf("Usage:\n<program_name> <rows> <columns> <aug_rows> <aug_columns>\n");
return -1;
}
int r1=atoi(argv[1]);
int c1=atoi(argv[2]);
int r2=atoi(argv[3]);
int c2=atoi(argv[4]);
if(r1 != r2) {
printf("\n Number of rows of both matrix should be same %d != %d", r1, r2);
return -2;
}
int **matrix1 = createMatrix(r1,c1);
int **matrix2 = createMatrix(r2,c2);
printf("Matrix 1 :\n");
initMatrix(matrix1, r1,c1);
displayMatrix(matrix1, r1,c1);
printf("Matrix 2 :\n");
initMatrix(matrix2, r2,c2);
displayMatrix(matrix2, r2,c2);
int **aug_matrix3 = augMatrix(matrix1, r1, c1, matrix2, r2, c2);
deleteMatrix(matrix1,r1,c1);
deleteMatrix(matrix2,r2,c2);
int r3 = r1;
int c3 = c1 + c2;
printf("\nSum Matrix 3 :\n");
displayMatrix(aug_matrix3, r3, c3);
deleteMatrix(aug_matrix3,r3,c3);
return 0;
}
void turnRightAngle(int direction)
{
curAng = angle;
while (abs(angle-curAng) < abs(11900)) {
displayMatrix("LEFT");
//printf("angle: %d\tcurAngle: %d\tangle-curAngle: %d\r\n", angle, curAng, angle-curAng);
targetSpeedW = direction*10;
}
}
int main(void) {
void scalarMultiply(int nRows, int nCols, int matrix[nRows][nCols],
int scalar);
void displayMatrix(int nRows, int nCols, int martix[nRows][nCols]);
int sampleMatrix[3][5] = {
{7, 16, 55, 13, 12}, {12, 10, 52, 0, 7}, {-2, 1, 2, 4, 9}};
printf("Original matrix:\n");
displayMatrix(3, 5, sampleMatrix);
scalarMultiply(3, 5, sampleMatrix, 2);
printf("Matrix multipled by 2:\n");
displayMatrix(3, 5, sampleMatrix);
scalarMultiply(3, 5, sampleMatrix, -1);
printf("Martix multiplied by -1:\n");
displayMatrix(3, 5, sampleMatrix);
return 0;
}
void
MainWindow::binarize()
{
// auto adaptiveMethod = _gaussian->isChecked() ?
// cv::ADAPTIVE_THRESH_GAUSSIAN_C : cv::ADAPTIVE_THRESH_MEAN_C;
cv::threshold(_src_gray, _dst, _cSlider->value(), 255, cv::THRESH_BINARY);
// cv::adaptiveThreshold(_src_gray, _dst, 255,
// adaptiveMethod,
// cv::THRESH_BINARY,
// _blockSize->value(), _cSlider->value());
deleteTooSmallClusters();
displayMatrix(_dst);
calculatePercent();
}
void displayInternalInfoOfASTNode(ASTNode *node){
if (node == NULL) {
return;
} else {
/* show type and subtype */
printf("type: %s - %s\n",typeList[node->type],subTypeList[node->subType]);
displayMatrix(node->mat);
/* show more detail */
switch (node->type) {
case Num:
printf("scalarValue: %f\n", node->scalarValue);
break;
case VectorExp:
printf("node->start:%f, node->step:%f, node->end:%f\n",node->start,node->step,node->end);
default:
break;
}
}
}
int SubSetSum(int A[], int n, int T)
{
int i, j, M[20][100];
M[0][0] = 0;
for(j = 1; j <= T; j++)
M[0][j] = 0;
for(i = 1; i <= n; i++)
M[i][0] = 1;
for(i = 1; i <= n; i++)
for(j = 1; j <= T; j++)
if(j >= A[i - 1])
M[i][j] = M[i - 1][j] || M[i][j - A[i - 1]];
else
M[i][j] = M[i - 1][j];
displayMatrix(M, n, T);
return M[n][T];
}
Ref decomposition_call(ref_list args){
if (args->length != 1){
set_err(ETYPE, "too many arguments");
return NULL;
}
if (arg_isMatrix(args->list[0]) == false) return NULL;
Matrix M = CAST_REF2MATRIX(args->list[0]);
Matrix LUP[3] = {NULL, NULL, NULL};
decomposition(M,&LUP[0],&LUP[1],&LUP[2]);
int i;
for ( i = 0; i < 3; i++){
if (LUP[i] != NULL){
displayMatrix(LUP[i]);
dropMatrix(LUP[i]);
}
}
return NO_REF;
}
void powerIteration (double **M, const int &n, const double &beta)
{
bool ok = 0;
int steps = 0, i;
double **c = new double*[n];
for (i = 0; i < n; i++)
c[i] = new double[1];
double **r = createVector_R (n);
//
printf (" Eigenvector r:\n");
displayMatrix (r, n, 1);
printf ("\n\t Power Iteration:\n\n");
//
while (!ok)
{
c = matrixMultiply (M, r, c, n);
ok = 1;
for (i = 0; i < n; i++)
{
double x = floor(c[i][0] * eps + 0.5) / eps;
double y = floor(r[i][0] * eps + 0.5) / eps;
if (x != y)
{
ok = 0;
i = n;
}
}
steps++;
//
printf("\n%2d) ", steps);
//
for (i = 0; i < n; i++)
{
//
printf(" %.3f ", c[i][0]);
//
r[i][0] = c[i][0];
}
}
printf ("\n\n\n\t>>>>> Damping factor: %.3f <<<<<", beta);
printf("\n\n\tPAGE RANK SCORE [after %d iterations]:\n\n", steps);
for (i = 0; i < n; i++)
{
printf(" - PageRank Score for node %d is: %.3f\n", i+1, r[i][0]);
}
deallocMatrix <double> (M, n);
deallocMatrix <double> (r, n);
deallocMatrix <double> (c, n);
printf("\n-------------------------------------------------------------------------\n\n\n\n");
}
void displayErr(int err) {
char str[5];
snprintf(str, 5, "E%03d", err);
displayMatrix(str);
}
int main(void)
{
DDRA = 0xF0; PORTA = 0x0F;
DDRB = 0x00; PORTB = 0xFF;
DDRC = 0xFF; PORTC = 0x00;
DDRD = 0xFF; PORTD = 0x00;
gameSelector = 0xFF;
LINE_init();
TimerSet(1);
TimerOn();
srand(0);
InitADC();
initUSART(1);
while (1)
{
if(SandM_period >= 10)
{
if(USART_HasReceived(1)){
gameSelector = receiveData(1);
}
SandM_period = 0;
}
++SandM_period;
if(gameSelector == 0x02)
{
if(TRLINES_period >= 9)
{
TRLINES_period = 0;
}
if(TRuser_period >= 80)
{
TunRun_ChkLoss();
TunRun_JoyStk();
TUNRUN_LINES();
TunRun_Input();
TRuser_period = 0;
}
displayMatrix();
++TRLINES_period;
++TRuser_period;
}
if(gameSelector == 0x04)
{
if(arrows_period >= 750)
{
DDR_arrows();
if((arrow == 0) || (arrow == 2))
{
ADMUX = 0x40;
}
else
{
ADMUX = 0x41;
}
arrows_period = 0;
}
if(arrows_inputP >= 20)
{
DDR_Input();
DDR_JoyStk();
arrows_inputP = 0;
}
if(arrowsPeriod >= 250)
{
DDR_ChkLoss();
arrowsPeriod = 0;
}
displayMatrix();
++arrows_inputP;
++arrows_period;
++arrowsPeriod;
}
if(gameSelector == 0x03)
{
if(snakeJoystick > 30)
{
Snake_JoyStk();
Snake_Input();
snakeJoystick = 0;
}
if(body > body_period)
{
SnakeBody();
Snake_Movement();
body = 0;
}
++snakeJoystick;
++joystick;
++body;
displayMatrix();
}
while(!TimerFlag);
TimerFlag = 0;
}
}
/*
* ekf_slam 算法设计与实现
* 利用： 里程速度信息 + 观测landmark信息 --> 更新的机器人状态与地图信息
*
* 步骤
* 1. 状态初始化，初始化时间
* 2. 运动模型： 速度向量表（t-1） + dt
* 3. 观测模型： 新状态：miu + odservation
* 已有状态：代入融合更新
* 4. 数据融合： 预测与观测偏差-->卡尔曼增益
* 状态更新 --> robot pos + map postions
* 变量说明：
* 1. miu_state（3+2n,1） 系统状态量 miu_convar_p 系统状态协方差量（3+2n,3+2n）
* 2. observed_mark_num 观测到landmark总数量（n） observed_mark_num_old landmark已有储存量 -->状态扩维
* 3.
*/
void QrSlam::ekfSlam(float V, float W)
{
cout<<"ekfslam start !"<<endl;
if(init_EKF_value_)
{
ftime(&Time_);
miu_state = Mat::zeros(3,1,CV_32FC1); //已去掉跟s有关的项，原来是3+3*
miu_state.at<float>(0) = robot_info_.X + coordinate_x_;
miu_state.at<float>(1) = robot_info_.Y + coordinate_y_; // 取y 标准正向
miu_state.at<float>(2) = robot_info_.Theta + coordinate_angle_ ;
miu_convar_p = Mat::zeros(3,3,CV_32FC1); //这个可以放到初始化函数中去 ？？初值
miu_convar_p.at<float>(0,0) = 0.1;//0.1;//1;//100;//0.1;
miu_convar_p.at<float>(1,1) = 0.10;//0.1;//1;//100;//0.1;
miu_convar_p.at<float>(2,2) = 0.38;//0.1;//0.38;
init_EKF_value_ = false;
TimeOld_ = Time_;
velocities_.push_back(Point2f(V,W));
}
else
{
ftime(&Time_);
delta_t_ = (Time_.time - TimeOld_.time) + (Time_.millitm - TimeOld_.millitm)/1000.0; // 秒
/////----------------------------------------------------------------------------------------------------------
// landmark_vector_ = pQrDetect_->QrCodeMapping(visual_mark_num_,robot_pose); //获取的观测值是实际物理距离值
cout<<"observation start !"<<endl;
genObservations();
getNumQrcode();
/////----------------------------------------------------------------------------------------------------------
int observed_mark_num = observed_landmark_num.size();
cout<<"----------------"<<observed_mark_num_old<<endl;
if(observed_mark_num > observed_mark_num_old) //状态miu_SLAM 扩维 一个码两个量（x,y）加入系统状态 ？？？加入 mark(r,seita) 坐标值 扩维与加数。
{
//miu_SLAM扩维 后面将其改造成成员函数
cv::Mat miu_state_new = Mat::zeros(3+2*observed_mark_num,1,CV_32FC1); //已去掉跟s有关的项，
for(uint i=0;i<3+2*observed_mark_num_old;i++)
{
miu_state_new.at<float>(i) = miu_state.at<float>(i);
}
miu_state = Mat::zeros(3+2*observed_mark_num,1,CV_32FC1); //已去掉跟s有关的项，原来是3+3*
for(uint i=0;i<3+2*observed_mark_num_old;i++)
{
miu_state.at<float>(i) = miu_state_new.at<float>(i);
}
displayMatrix(miu_state);
// xP_SLAM 扩维
cv::Mat miu_convar_p_new = Mat::zeros(3+2*observed_mark_num,3+2*observed_mark_num,CV_32FC1); //这个可以放到初始化函数中去 ？？初值???
displayMatrix(miu_convar_p_new);
displayMatrix(miu_convar_p);
for(uint i=0;i<3+2*observed_mark_num_old ;i++)
{
for(uint j=0;j<3+2*observed_mark_num_old;j++)
{
miu_convar_p_new.at<float>(i,j) = miu_convar_p.at<float>(i,j);
}
}
miu_convar_p = Mat::zeros(3+2*observed_mark_num,3+2*observed_mark_num,CV_32FC1); //已去掉跟s有关的项，原来是3+3*
for(uint i=0;i<3+2*observed_mark_num_old;i++)
{
for(uint j=0;j<3+2*observed_mark_num_old;j++)
miu_convar_p.at<float>(i,j) = miu_convar_p_new.at<float>(i,j);
}
for(uint i=3+2*observed_mark_num_old;i<3+2*observed_mark_num;i++)
{
miu_convar_p.at<float>(i,i) = 1000000; //对角方差要大1000000
}
}
observed_mark_num_old = observed_mark_num;
// xPred_SLAM 与 xPPred_SLAM 传入值的问题
Mat miu_prediction = Mat::zeros(3+2*observed_mark_num, 1, CV_32FC1); //原来是3+3*N
//Point3f xLast; //已经不需要，第一次进来时用初始化的
Mat x_convar_p_prediction = Mat::zeros(3+2*observed_mark_num, 3+2*observed_mark_num, CV_32FC1);//协方差矩阵的估计矩阵
Mat Fx = Mat::zeros(3, 3+2*observed_mark_num, CV_32FC1);
Mat I_SLAM = Mat::eye(3+2*observed_mark_num, 3+2*observed_mark_num, CV_32FC1); //算法中的单位矩阵
Mat Gt = Mat::zeros(3+2*observed_mark_num, 3+2*observed_mark_num, CV_32FC1);
Mat Gt_temp = Mat::zeros(3, 3, CV_32FC1); //用来计算Gt的3*3矩阵
Mat Ht = Mat::zeros(2, 3+2*observed_mark_num, CV_32FC1);
Mat Kt = Mat::zeros(3+2*observed_mark_num, 2, CV_32FC1); //
Mat Fj;
Mat cal_temp = Mat::zeros(3, 1, CV_32FC1); //算法第三行计算xPred_SLAM时的3*1矩阵
Mat Qt = Mat::zeros(2, 2, CV_32FC1);
Mat Ht_temp = Mat::zeros(2, 5, CV_32FC1); //用来计算Ht的2*5矩阵
// Mat miu_temp_sum=Mat::zeros(3+2*observed_mark_num,1,CV_32FC1); //用来计算Ht的2*5矩阵
// Mat Kt_i_Ht_sum=Mat::zeros(3+2*observed_mark_num,3+2*observed_mark_num,CV_32FC1);
// Mat delta_z_observed =Mat::zeros(3+2*observed_mark_num,3+2*observed_mark_num,CV_32FC1);
Mat St = Mat::zeros(2,2,CV_32FC1); //vv
Mat Rt = Mat::zeros(3,3,CV_32FC1); //vv
Mat Vt = Mat::zeros(3,2,CV_32FC1);
Mat Mt = Mat::zeros(2,2,CV_32FC1);
landmark_miu_conver_p_ = Mat::zeros(2,2,CV_64FC1); //协方差椭圆有关
////////////////////////////////////////////////////////////////////////////////////
// motionModelIndex==0 //Velocity模式
velocities_.push_back(Point2f(V,W));
Point2f VEL = velocities_.at(velocities_.size()-2); //数组从0开始 又存入一值
Vd_ = VEL.x;
Wd_ = VEL.y; // 取y 标准正向
cout<<"Vd: "<<Vd_<<" "<<"Wd"<<Wd_<<endl;
if( Wd_ < 0.00006 && Wd_ >=0) Wd_ = 0.00006;
if( Wd_ > -0.00006 && Wd_ <0) Wd_ = -0.00006;
cout<<"vd/wd"<<Vd_/Wd_<<endl;
// float Vr_noise, Wr_noise, Sigv, Sigw;
// Sigv = a1*Vd_*Vd_+a2*Wd_*Wd_;
// Sigw = a3*Vd_*Vd_+a4*Wd_*Wd_;
// Vr_noise = genGaussianValue(Sigv);
// Wr_noise = genGaussianValue(Sigw);
// Vd_ = VEL.x + Vr_noise;
// Wd_ = VEL.y + Wr_noise;
// if( Wd_ <0.00006 && Wd_ >=0) Wd_ =0.0001;
// if( Wd_ >-0.00006 && Wd_ <0) Wd_ =-0.0001;
////基于EKF的SLAM方法， 条件有当前观测量Observations， 上一时刻估计所得机器人位置
//计算Ft
Fx.at<float>(0,0) = 1.0;
Fx.at<float>(1,1) = 1.0;
Fx.at<float>(2,2) = 1.0;
//计算cal_temp，预测不加扰动
float last_miu_theta = miu_state.at<float>(2) ;//上一个miu_SLAM的角度
angleWrap(last_miu_theta);
//speed mode motion increase Wd 不能为 0
cal_temp.at<float>(0) = -Vd_/Wd_*sin(last_miu_theta) + Vd_/Wd_*sin(last_miu_theta+Wd_*delta_t_);
cal_temp.at<float>(1) = Vd_/Wd_*cos(last_miu_theta) - Vd_/Wd_*cos(last_miu_theta+Wd_*delta_t_);
cal_temp.at<float>(2) = Wd_*delta_t_;
cout<<"cal_temp"<<cal_temp<<endl;
///prediction
miu_prediction = miu_state + Fx.t()*cal_temp; // X'= X +Jaci_f(x)*delt(x) predicted mean
angleWrap(miu_prediction.at<float>(2));
fre<<cal_temp.at<float>(0)<<" "<<cal_temp.at<float>(1)<<" "<< cal_temp.at<float>(2)<<endl;
// cout<<"miu_prediction"<<miu_prediction<<endl;
//计算Gt Jacibi_x(x,y,theita)
Gt_temp.at<float>(0,2) = -Vd_/Wd_*cos(last_miu_theta) + Vd_/Wd_*cos(last_miu_theta+Wd_*delta_t_);
Gt_temp.at<float>(1,2) = -Vd_/Wd_*sin(last_miu_theta) + Vd_/Wd_*sin(last_miu_theta+Wd_*delta_t_);
Gt=I_SLAM+Fx.t()*Gt_temp*Fx ;
//change()
// Gt_temp.at<float>(0,2) = Vd_/Wd_*cos(last_miu_theta) - Vd_/Wd_*cos(last_miu_theta+Wd_*delta_t_);
// Gt_temp.at<float>(1,2) = Vd_/Wd_*sin(last_miu_theta) - Vd_/Wd_*sin(last_miu_theta+Wd_*delta_t_);
// Gt=I_SLAM+Fx.t()*Gt_temp*Fx ;
//计算Vt Jacibi_u(v,w)
Vt.at<float>(0,0) = (-sin(last_miu_theta)+sin(last_miu_theta+Wd_*delta_t_))/Wd_; Vt.at<float>(0,1)=Vd_*(sin(last_miu_theta)-sin(last_miu_theta+Wd_*delta_t_))/Wd_/Wd_+Vd_*cos(last_miu_theta+Wd_*delta_t_)*delta_t_/Wd_;
Vt.at<float>(1,0) = (cos(last_miu_theta)-cos(last_miu_theta+Wd_*delta_t_))/Wd_; Vt.at<float>(1,1)=-Vd_*(cos(last_miu_theta)-cos(last_miu_theta+Wd_*delta_t_))/Wd_/Wd_+Vd_*sin(last_miu_theta+Wd_*delta_t_)*delta_t_/Wd_;
Vt.at<float>(2,0) = 0; Vt.at<float>(2,1)=delta_t_;
//计算Mt motion noise ; why add the motion noise ?????
Mt.at<float>(0,0) = a1*Vd_*Vd_+a2*Wd_*Wd_;
Mt.at<float>(1,1) = a3*Vd_*Vd_+a4*Wd_*Wd_;
Rt = Vt*Mt*Vt.t();//计算Rt
//计算预测方差矩阵miu_convar_p
cout<<"----------------------------------------"<<endl;
// cout<<"miu_convar_p"<<miu_convar_p<<endl;
x_convar_p_prediction = Gt*miu_convar_p*Gt.t() + Fx.t()*Rt*Fx; //计算预测方差 Px ?????????? xP_SLAM 与 xPred_SLAM
//计算Qt
Qt.at<float>(0,0) = sigma_r*sigma_r;
Qt.at<float>(1,1) = sigma_phi*sigma_phi;
/////----------------------------------------------------------------------------------------------------------
// LandmarkVector = pLocalizer->QrCodeMapping(MarkVisualNum);
// GenObservations();
///？？观测后面要改成 x y 坐标系
// /////----------------------------------------------------------------------------------------------------------
/////----------------------------------------------------------------------------------------------------------
//更新
int j = 0; // the order of Qrmark 表示观察到的 特征标记 与状态量中landmark有关
int Qid = 0; // the Id of Qrmark
int observed_flag = 0 ;//是否观察到过的标志 0表示从未出现过
float dst;
float theta;
float q;
Point2f delta;
Point2f z,zp;
Mat delta_z;
for(int i=0; i< observations_.size(); i++)
{
Qid = observations_.at(i).z;
z = Point2f(observations_.at(i).x,observations_.at(i).y); //观测值 是极坐标
for(int c=0; c<Lm_observed_Num.size(); c++)
{
if(Qid == Lm_observed_Num[c]) // 出现过 逐一比对已经观察到的mark列表
{
observed_flag = 1 ;
j = c; // 选取第j个landmark导入观察模型并进行状态更新。
}
}
if(observed_flag == 0) //从未出现过
{
// landmark_observed.at<float>(i)=j ;//保存住这次观测到的路标号
Lm_observed_Num.push_back(Qid) ;
j = Lm_observed_Num.size()-1 ; // 注意 数组从0开始 -1 ？？？？？？？？？？
// 在观测函数 x y 极坐标系下的值。
miu_prediction.at<float>(2*j+3) = miu_prediction.at<float>(0) + z.x*cos( z.y + miu_prediction.at<float>(2) ); //第j个landmark的y坐标
miu_prediction.at<float>(2*j+4) = miu_prediction.at<float>(1) + z.x*sin( z.y + miu_prediction.at<float>(2) ); //第j个landmark的x坐标
}
observed_flag=0 ;
//预测测量值 ？？？？？？反向查找运动预测中 Point2f(xPred_SLAM.at<float>(2*j+3),xPred_SLAM.at<float>(2*j+4))
// 不然就为加入地图mark 无变化 xPred_SLAM 都为世界坐标系下值
//这里的z相当于landmark的标号 （全局坐标下： 地图值- 预测机器人值）与 观测值 z ： 表示mark与robot的相对距离
delta = Point2f(miu_prediction.at<float>(2*j+3),miu_prediction.at<float>(2*j+4))-Point2f(miu_prediction.at<float>(0),miu_prediction.at<float>(1));
q = delta.x*delta.x+delta.y*delta.y ;
dst = sqrt(q);
theta = atan2(delta.y, delta.x) - miu_prediction.at<float>(2); //偏离robot的方向角方向的角度 相对xy坐标系下值
zp.x= dst;
angleWrap(theta);
zp.y= theta;
//计算Fj
Fj=Mat::zeros(5,3+2*observed_mark_num,CV_32FC1); //已降维，本来是6行
Fj.at<float>(0,0) = 1;
Fj.at<float>(1,1) = 1;
Fj.at<float>(2,2) = 1;
Fj.at<float>(3,2*j+3) = 1;
Fj.at<float>(4,2*j+4) = 1;
//计算Htjaccibi
Ht_temp.at<float>(0,0) = -delta.x*dst;
Ht_temp.at<float>(0,1) = -delta.y*dst;
Ht_temp.at<float>(0,3) = delta.x*dst;
Ht_temp.at<float>(0,4) = delta.y*dst;
Ht_temp.at<float>(1,0) = delta.y;
Ht_temp.at<float>(1,1) = -delta.x;
Ht_temp.at<float>(1,2) = -q;
Ht_temp.at<float>(1,3) = -delta.y;
Ht_temp.at<float>(1,4) = delta.x;
///~~~~~~~~~~~~~~~~~~
Ht=(1/q)*Ht_temp*Fj ;
St = Ht*x_convar_p_prediction*Ht.t()+Qt;
Kt = x_convar_p_prediction*Ht.t()*St.inv();
z = z-zp; // 更新的处理
angleWrap(z.y);
delta_z = Mat::zeros(2,1,CV_32FC1);
delta_z.at<float>(0) = z.x;
delta_z.at<float>(1) = z.y;
//×× //xPred_SLAM为xy坐标系 delta_z 为极坐标系 存在点问题 观测模型是以 极坐标建立的 H阵描述的也就是 极值与极径的关系 即K表述成立
// miu_temp_sum = miu_temp_sum + Kt*delta_z ;
// Kt_i_Ht_sum = Kt_i_Ht_sum + Kt*Ht;
// }
// miu_prediction =miu_prediction + miu_temp_sum ; // xPred_SLAM 关于landmark为极坐标值
// angleWrap(miu_prediction.at<float>(2));
// x_convar_p_prediction= (I_SLAM-Kt_i_Ht_sum)*x_convar_p_prediction;
// miu_state=miu_prediction;
// miu_convar_p=x_convar_p_prediction;
//×××
////××
miu_prediction = miu_prediction +Kt*delta_z; // xPred_SLAM 关于landmark为极坐标值
angleWrap(miu_prediction.at<float>(2));
x_convar_p_prediction= (I_SLAM-Kt*Ht)*x_convar_p_prediction;
}
miu_state=miu_prediction;
miu_convar_p=x_convar_p_prediction;
////×
///
Point3f xPred;
xPred.x=miu_prediction.at<float>(0);
xPred.y=miu_prediction.at<float>(1);
xPred.z=miu_prediction.at<float>(2);
angleWrap(xPred.z);
est_path_points_.push_back(xPred);
miu_convar_p= 0.5*(miu_convar_p+miu_convar_p.t());
// xP_SLAM(Rect(0,0,3,3)).copyTo(xP); //分离出位置的协方差，用于图形显示不确定椭圆
//draw_mark();
for(int t=0;t<3+2*Lm_observed_Num.size();t++)
{
cout <<" "<<miu_state.at<float>(t)<<" ";
}
cout <<" "<<endl;
// cvWaitKey(10);
TimeOld_ = Time_;
}
}
void
computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations, std::vector<double>& zs)
{
double const gamma = 1.0;
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
Matrix3x3d zero3x3d;
makeZeroMatrix(zero3x3d);
Matrix4x4d zeroQuat;
makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;
double const denomT = 1.0 / (1.0 + nRelPoses);
vector<double> denomQ(nViews, 1.0); // from the psd constraint
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
denomQ[i] += 1;
denomQ[j] += 1;
}
for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];
double T = 0.0;
vector<double> T1(nRelPoses);
vector<double> ZT1(nRelPoses, 0.0);
double T2;
double ZT2 = 0;
vector<Matrix4x4d> Q(nViews, zeroQuat);
vector<Matrix4x4d> Q1(nViews, zeroQuat);
vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);
vector<Matrix3x3d> A(nRelPoses, zero3x3d);
vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);
for (int iter = 0; iter < nIterations; ++iter)
{
// Convex hull of rotation matrices
for (int i = 0; i < nViews; ++i)
{
Matrix4x4d q = Q[i] + ZQ1[i];
if (i > 0)
projectConvHull_SO3(q);
else
{
makeZeroMatrix(q);
q[0][0] = 1;
}
Q1[i] = q;
addMatricesIP(Q[i] - q, ZQ1[i]);
} // end for (i)
// Shrinkage of T (we want to minimize T)
T2 = std::max(0.0, T + ZT2 - gamma);
ZT2 += T - T2;
// Cone constraint
for (int k = 0; k < nRelPoses; ++k)
{
double t = T + ZT1[k];
Matrix3x3d a = A[k] + ZA1[k];
proxDataResidual_Frobenius(sigma, t, a);
T1[k] = t;
ZT1[k] += T - t;
A1[k] = a;
addMatricesIP(A[k] - a, ZA1[k]);
} // end for (k)
// Enforce linear consistency
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
Matrix4x4d qi = Q[i] + ZQ2i[k];
Matrix4x4d qj = Q[j] + ZQ2j[k];
Matrix3x3d a = A[k] + ZA2[k];
proxConsistency(relativeRotations[k], qi, qj, a);
Q2i[k] = qi;
Q2j[k] = qj;
A2[k] = a;
addMatricesIP(Q[i] - qi, ZQ2i[k]);
addMatricesIP(Q[j] - qj, ZQ2j[k]);
addMatricesIP(A[k] - a, ZA2[k]);
} // end for (k)
// Averaging of the solutions
for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i];
T = T2 - ZT2;
for (int k = 0; k < nRelPoses; ++k) T += T1[k] - ZT1[k];
T *= denomT;
T = std::max(0.0, T);
for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k];
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]);
addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]);
addMatricesIP(A2[k] - ZA2[k], A[k]);
} // end for (k)
for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]);
for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]);
if ((iter % 500) == 0)
{
cout << "iter: " << iter << " t = " << T << endl;
cout << "T1 = "; displayVector(T1);
cout << "ZT1 = "; displayVector(ZT1);
cout << "T2 = " << T2 << " ZT2 = " << ZT2 << endl;
Matrix<double> ZZ(4, 4);
for (int i = 0; i < nViews; ++i)
{
copyMatrix(Q[i], ZZ);
SVD<double> svd(ZZ);
cout << "Q = "; displayMatrix(ZZ);
cout << "SV = "; displayVector(svd.getSingularValues());
//Matrix3x3d R = getRotationFromQuat(Q[i]);
//cout << "R = "; displayMatrix(R);
} // end for (i)
}
} // end for (iter)
rotations.resize(nViews);
for (int i = 0; i < nViews; ++i)
rotations[i] = getRotationFromQuat(Q[i]);
zs = ZT1;
} // end computeConsistentRotations_Linf()
int main(void) {
XGpio dip, push;
XScuTimer Timer; /* Cortex A9 SCU Private Timer Instance */
XScuTimer_Config *ConfigPtr;
int value, skip, psb_check, dip_check, status, timerCounter, time1, time2;
VectorArray AInst;
VectorArray BTinst;
VectorArray PInst;
xil_printf("-- Start of the Program --\r\n");
xil_printf("Enter choice: 1 (SW->Leds), 2 (Timer->Leds), 3 (Matrix), 4 (Exit) \r\n");
XGpio_Initialize(&dip, XPAR_SW_8BIT_DEVICE_ID);
XGpio_SetDataDirection(&dip, 1, 0xffffffff);
XGpio_Initialize(&push, XPAR_BTNS_5BIT_DEVICE_ID);
XGpio_SetDataDirection(&push, 1, 0xffffffff);
ConfigPtr = XScuTimer_LookupConfig (XPAR_PS7_SCUTIMER_0_DEVICE_ID);
status = XScuTimer_CfgInitialize (&Timer, ConfigPtr, ConfigPtr->BaseAddr);
if(status != XST_SUCCESS){
xil_printf("Timer init() failed\r\n");
return XST_FAILURE;
}
// Load timer with delay
XScuTimer_LoadTimer(&Timer, ONE_SECOND);
// Set AutoLoad mode
XScuTimer_EnableAutoReload(&Timer);
while (1) {
xil_printf("CMD:> ");
// Read an input value from the console.
value = inbyte();
skip = inbyte(); //CR
skip = inbyte(); //LF
switch (value) {
case '1':
while(!XGpio_DiscreteRead(&push, 1))
{
dip_check = XGpio_DiscreteRead(&dip, 1);
LED_IP_mWriteReg(XPAR_LED_IP_S_AXI_BASEADDR, 0, dip_check);
for (skip = 0; skip < 9999999; skip++);
}
break;
case '2':
timerCounter = 0;
XScuTimer_Start(&Timer);
while(!XGpio_DiscreteRead(&push, 1))
{
if(XScuTimer_IsExpired(&Timer))
{
XScuTimer_ClearInterruptStatus(&Timer);
timerCounter = (timerCounter + 1) % 256;
LED_IP_mWriteReg(XPAR_LED_IP_S_AXI_BASEADDR, 0, timerCounter);
}
}
break;
case '3':
setInputMatrices(AInst, BTinst);
displayMatrix(AInst);
displayMatrix(BTinst);
XScuTimer_Start(&Timer);
// Software matrix
time1 = XScuTimer_GetCounterValue(&Timer);
multiMatrixSoft(AInst, BTinst, PInst);
time2 = XScuTimer_GetCounterValue(&Timer);
xil_printf("SW time: %d\n\n", time1-time2);
displayMatrix(PInst);
// Hardware matrix
time1 = XScuTimer_GetCounterValue(&Timer);
multiMatrixHard(AInst, BTinst, PInst);
time2 = XScuTimer_GetCounterValue(&Timer);
XScuTimer_Stop(&Timer);
xil_printf("HW time: %d\n\n", time1-time2);
displayMatrix(PInst);
break;
case '4':
// Exit
return XST_SUCCESS;
break;
default :
break;
}
}
}
int main(int argc, char **argv)
{
int tabMatrixId[4][4] =
{
{1, 0, 0, 0},
{0, 1, 0, 0},
{0, 0, 1, 0},
{0, 0, 0, 1}
};
int tabMatrixAn[4][4];
int tabGenerator[4][8];
int *tabFile, *tabOctect, *tabChiffrer = NULL, *temp = NULL;
int answerCode, answerFile;
FILE *fichier = NULL;
int taille=0, length=0, i=0, j=0, k=0;
char octetActuel = 0;
do
{
askMatrix(tabMatrixAn);
}while(verifMatrixNull(tabMatrixAn) == 0 || verifMatrixDuplicate(tabMatrixAn) == 0);
displayMatrix(tabMatrixId);
displayMatrix(tabMatrixAn);
displayGenerator(tabMatrixId, tabMatrixAn, tabGenerator);
do
{
choiceMatrix(&answerCode);
}while(answerCode !=1 && answerCode != 2);
fichier = fopen(argv[1], "rb");
fseek(fichier, 0, SEEK_SET);
tabFile = malloc(8 * taille * sizeof(int));
while(fread(&octetActuel, sizeof(octetActuel), 1, fichier) != 0)
{
tabOctect = calculBin(octetActuel);
for(j=0; j<8; j++)
{
tabFile[i*8+j]=tabOctect[j];
}
i++;
free(tabOctect);
}
fclose(fichier);
if(answerCode==1)
{
temp = malloc(8 * sizeof(int));
for(i = 0; i < (taille / 4); i++)
{
for(j = 0 ; j < 8 ; j++)
temp[j] = 0;
for(j = 0; j < 4 ; j++)
{
if(tabFile[(i * 4) + j] == 1)
{
for(k = 0; k < 8; k++)
{
if(tabGenerator[j][k] == 1)
temp[k] = (temp[k] == 0) ? 1 : 0;
}
}
}
for(j = 0; j < 8; j++)
{
tabChiffrer = realloc(tabChiffrer, ++length * sizeof(int));
tabChiffrer[i*8+j] = temp[j];
}
}
free(temp);
}
else
{
printf("La matrice pour dechiffrer n'est pas correcte.\n");
}
fichier = fopen("nouveauFichier", "wb");
temp = malloc(8*sizeof(int));
for(i=0; i<length; i+=8)
{
for(j=0; j<8; j++)
{
temp[j] = tabChiffrer[i*8+j];
}
octetActuel = calculOct(temp);
fwrite(&octetActuel, sizeof(octetActuel), 1, fichier);
}
free(temp);
if (fichier!=0 && answerCode==1)
printf("\n\nNouveau fichier code a ete cree!\n\n");
fclose(fichier);
return 0;
}
int main(int argc, char const *argv[])
{
int matrixSize = strtol(argv[1], NULL, 10);
int coreCount = omp_get_num_procs();
int threadCount = strtol(argv[2], NULL, 10);
double startTime, finishTime;
double **a_augmented, **a; // n x n Matrix as a 2D array
double diagonalElement, bestElement, factor;
int bestRowIndex = 0; // used in partial pivoting (index of row having greatest absolute value)
int i, j, k; // for loop counters
double *x; // Solutions
double *b;
printf("Matrix Size: %d\n", matrixSize);
printf("Number of Cores: %d\n", coreCount);
#pragma omp parallel num_threads(threadCount)
{
if (omp_get_thread_num() == 0)
printf("Thread Count: %d\n", omp_get_num_threads());
}
// Start Timer
startTime = omp_get_wtime();
// Allocate memory
// a_augmented will be the augmented matrix
a_augmented = (double **) malloc(matrixSize * sizeof(double *));
// a will be the randomly generated matrix
a = (double **) malloc(matrixSize * sizeof(double *));
x = (double *) malloc(matrixSize * sizeof(double));
b = (double *) malloc(matrixSize * sizeof(double));
if (DEBUG == 1)
Read_matrix(&a, &a_augmented, matrixSize);
else
Gen_matrix(&a, &a_augmented, matrixSize, threadCount);
// a will not be modified after this point
// Only the a_augmented will be modified
// Display generated matrix:
displayMatrix(a, matrixSize);
for (i = 0; i < matrixSize - 1; ++i)
{
// Partial Pivoting:
// the algorithm selects the entry with largest absolute value from
// the column of the matrix that is currently being considered as
// the pivot element.
// Diagonal Element
diagonalElement = a_augmented[i][i];
// debug_printf("diagonalElement%d = %f\n", i, diagonalElement);
// Find the best row (the one with the largest absolute value in the
// column being worked on)
bestRowIndex = i;
bestElement = diagonalElement;
for (j = i + 1; j < matrixSize; ++j)
{
if (fabs(a_augmented[j][i]) > fabs(bestElement))
{
bestRowIndex = j;
bestElement = a_augmented[j][i];
// debug_printf("bestElement = %f\n", a_augmented[j][i]);
}
}
// Swap the rows
if (i != bestRowIndex)
{
// debug_printf("Row %d needs to be swapped with Row %d\n", i, bestRowIndex );
swapRow(&a_augmented[i], &a_augmented[bestRowIndex]);
// Update the diagonal element
diagonalElement = a_augmented[i][i];
// debug_printf("diagonalElement%d = %f\n", i, diagonalElement);
// displayMatrix(a_augmented, matrixSize);
}
// End of Partial Pivoting
// To make the diagonal element 1,
// divide the whole row with the diagonal element
// debug_printf("Row %d = Row %d / %f\n", i, i, diagonalElement);
for (j = 0; j < matrixSize + 1; ++j)
{
a_augmented[i][j] = a_augmented[i][j] / diagonalElement;
}
// Force the diagonal to be 1 (to avoid any roundoff errors in dividing above)
a_augmented[i][i] = 1;
diagonalElement = 1;
// debug_printf("Annihilation of column %d...\n", i);
// Annihilation: Zero all the elements in the column below the diagonal element
#pragma omp parallel for num_threads(threadCount) \
default(none) private(j, factor, k) shared(i, matrixSize, a_augmented)
for (j = i + 1; j < matrixSize; ++j)
{
// sleep(1);
factor = a_augmented[j][i];
if (factor != 0)
{
// debug_printf("Row %d = Row %d - %f*Row %d\n", j, j, factor, i);
for (k = i; k < matrixSize + 1; ++k)
{
a_augmented[j][k] = a_augmented[j][k] - factor * a_augmented[i][k];
}
// displayAugmentedMatrix(a, matrixSize);
}
}
}
// Make the diagonal element of the last row 1
a_augmented[matrixSize-1][matrixSize] = a_augmented[matrixSize-1][matrixSize] / a_augmented[matrixSize-1][matrixSize-1];
a_augmented[matrixSize-1][matrixSize-1] = 1;
// Display augmented matrix:
displayMatrix(a_augmented, matrixSize);
// Back substitution (parallelized)
backSubstitution(&a_augmented, matrixSize, threadCount);
// Record the finish time
finishTime = omp_get_wtime();
displayMatrix(a_augmented, matrixSize);
// Matrix X from augmented matrix
// Vector b from matrix A
for (i = 0; i < matrixSize; ++i)
{
x[i] = a_augmented[i][matrixSize];
b[i] = a[i][matrixSize];
}
// Find I^2 norm
iSquaredNorm(&a, x, b, matrixSize, threadCount);
// Print the time taken
printf("Time taken = %f\n", finishTime - startTime);
// Free memory
for (i = 0; i < matrixSize; ++i)
{
free(a[i]);
free(a_augmented[i]);
}
free(a);
free(a_augmented);
free(x);
free(b);
return 0;
}
int main(void)
{
int i;
Systick_Configuration();
LED_Configuration();
button_Configuration();
usart1_Configuration(9600);
SPI_Configuration();
TIM4_PWM_Init();
Encoder_Configration();
buzzer_Configuration();
ADC_Config();
//curSpeedX = 0;
//curSpeedW = 0;
//shortBeep(2000, 8000);
while(1) {
readSensor();
readGyro();
readVolMeter();
printf("LF %d RF %d DL %d DR %d aSpeed %d angle %d voltage %d lenc %d renc %d\r\n", LFSensor, RFSensor, DLSensor, DRSensor, aSpeed, angle, voltage, getLeftEncCount(), getRightEncCount());
displayMatrix("UCLA");
setLeftPwm(100);
setRightPwm(100);
delay_ms(1000);
}
//forwardDistance(4000,0,0,true);
//displayMatrix("Sped");
//targetSpeedX = 100;
//delay_ms(2000);
//
//targetSpeedX = 100;
//delay_ms(2000);
//printf("==============================================\n\r=======================================================\r\n");
//delay_ms(1000);
//displayMatrix("Wat");
//delay_ms(1000);
//displayMatrix("STOP");
//displayMatrix("GO");
turnRightAngle(LEFT);
targetSpeedW = 0;
delay_ms(1000);
turnRightAngle(RIGHT);
targetSpeedW = 0;
delay_ms(1000);
displayMatrix("STOP");
delay_ms(3000);
targetSpeedX = 0;
//
targetSpeedW = 10;
delay_ms(1000);
targetSpeedX = 0;
targetSpeedW = 0;
return 0;
}
