⭐ 欢迎来到虫虫下载站! | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站

📄 total_power3.c

📁 该程序用粒子滤波的方法跟踪一个机器人并根据WiFi信号修改位置信息。
💻 C
📖 第 1 页 / 共 4 页
字号:
                
                Thits32                = fct_caltpts(Phitijk, Phitjk, planes , nplanes , material , &tflag32 , Phit_nt);
                
                Thits33                = fct_caltpts(Phitjk, Phitk, planes , nplanes , material , &tflag33 , Phit_nt);
                
                Thits34                = fct_caltpts(Phitk, RX , planes , nplanes , material , &tflag34 , Phit_nt);
                
                
                nb_pts                 = (tflag31+tflag32+tflag33+tflag34+5);
                
                path3                  = mxMalloc(nb_pts*5*sizeof(double));
                
                
                
                path3[0]               = TX[0];
                
                path3[1]               = TX[1];
                
                path3[2]               = TX[2];
                
                path3[3]               = 0.0;
                
                path3[4]               = 0.0;
                
                
                indice                 = (tflag31+1)*5;
                
                
                for(j = 5 ; j < indice ; j++)
                    
                {
                    
                    path3[j]            = Thits31[j - 5];
                    
                }
                
                
                
                path3[0 + indice]       = Phitijk[0];
                
                path3[1 + indice]       = Phitijk[1];
                
                path3[2 + indice]       = Phitijk[2];
                
                path3[3 + indice]       = rpath3[3 + id];
                
                path3[4 + indice]       = rpath3[4 + id];
                
                
                ind                     = (tflag31+2)*5;
                
                indice                  = (tflag31+tflag32 + 2)*5;
                
                
                for(j = ind; j <indice ; j++)
                    
                {
                    
                    path3[j] = Thits32[j - ind];
                    
                }
                
                
                
                path3[0 + indice]       = Phitjk[0];
                
                path3[1 + indice]       = Phitjk[1];
                
                path3[2 + indice]       = Phitjk[2];
                
                path3[3 + indice]       = rpath3[8 + id];
                
                path3[4 + indice]       = rpath3[9 + id];
                
                
                ind                     = (tflag31+tflag32+3)*5;
                
                indice                  = (tflag31+tflag32+tflag33+3)*5;
                
                
                for(j = ind; j < indice ; j++)
                    
                {
                    
                    path3[j] = Thits33[j - ind];
                    
                }
                
                
                
                path3[0 + indice]       = Phitk[0];
                
                path3[1 + indice]       = Phitk[1];
                
                path3[2 + indice]       = Phitk[2];
                
                path3[3 + indice]       = rpath3[13 + id];
                
                path3[4 + indice]       = rpath3[14 + id];
                
                ind                     = (tflag31+tflag32+tflag33+4)*5;
                
                indice                  = (tflag31+tflag32+tflag33+tflag34+4)*5;
                
                
                for(j = ind ; j < indice; j++)
                    
                {
                    
                    path3[j] = Thits34[j - ind];
                    
                }
                
                
                
                
                path3[0 + indice]       = RX[0];
                
                path3[1 + indice]       = RX[1];
                
                path3[2 + indice]       = RX[2];
                
                path3[3 + indice]       = 0.0;
                
                path3[4 + indice]       = 0.0;
                
                
                compute_distloss(path3 , nb_pts , planes , material , fc , flp_scale  , currentpath , distance , lossfac);
                
                
                if(Thits31 != NULL)
                {
                    free(Thits31);
                    
                    Thits31 = NULL;
                    
                }
                
                if(Thits32 != NULL)
                {
                    free(Thits32);
                    
                    Thits32= NULL;
                    
                }
                
                if(Thits33 != NULL)
                {
                    free(Thits33);
                    
                    Thits33 = NULL;
                    
                }
                
                if(Thits34 != NULL)
                {
                    free(Thits34);
                    
                    Thits34 = NULL;
                    
                }
                
                if(path3 != NULL)
                {
                    
                    mxFree(path3);
                    
                    path3 = NULL;
                    
                }
                
            }
            
            
            
            // Compute final Value //
            

			temp = 0.0;
            
            for (i = 0 ; i < npath ; i++)
                
            {
                
                temp += (lossfac[i]*lossfac[i]);
                
            }
            

			rs_amp[n + ld] = sqrt(temp);


			if(rpath1 !=NULL)
			{
				
				mxFree(rpath1);
				
				rpath1 = NULL;
				
			}
			
			if(rpath2 !=NULL)
			{
				
				mxFree(rpath2);
				
				rpath2 = NULL;
				
				
			}
			
			if(rpath3 !=NULL)
			{
				
				mxFree(rpath3);
				
				rpath3 = NULL;
				
				
			}
			
			
			if(nr == 0 )
			{
				
				mxFree(path1);
				
				mxFree(path2);
				
				mxFree(path3);
				
				path1 = path2 = path3 = NULL;
				
			}
			
			
			if(nr == 1 )
			{
				
				mxFree(path2);
				
				mxFree(path3);
				
				path2 = path3 = NULL;
				
				
			}
			
			if(nr == 2 )
			{
				
				mxFree(path3);
				
				path3 = NULL;
				
			}
			
			
            if(distance != NULL)
			{
				
				mxFree(distance);

				distance = NULL;
				
			}
			
            if(lossfac != NULL)
			{
				
				mxFree(lossfac);

				lossfac = NULL;
				
			}          
            
        }
    

        
    }
    
    mxFree(TX_i);
    
    mxFree(TX_j);
    
    mxFree(TX_k);
    
    mxFree(Phiti);
    
    mxFree(Phitj);
    
    mxFree(Phitij);
    
    mxFree(Phitk);
    
    mxFree(Phitjk);
    
    mxFree(Phitijk);
    
    mxFree(Phit_nt);
    
    mxFree(TX);
    
    mxFree(RX);

        
}


/* ------------------------------------------------------------------------------------------------------------------------------------------------------------ */


void compute_distloss(double *path , int num_pts , double *planes , double *material , double fc , double flp_scale  , int currentpath , double *distance , double *lossfac)

{
    
    
    int j , jd , flagP2 , ind , ind1 , ind2;
    
    double P1P2x , P1P2y , P1P2z , dj , loss_jreal , loss_jimag  , epsreal, epsimag , uy;
    
    double lossreal = 1.0, lossimag = 0.0 , tmp1 , tmp2 , tmp;
    
    double Vd , vs , vs2;
    
    double x1 , x2 , y1 , y2 , x3 , y3 , x4 , y4 , x5 , y5 ,  r1 , r2;
    
    double lambda , Ao , cte = 1.0/flp_scale;
    
    
    lambda                = c/fc;
    
    Ao                    = lambda/(4.0*PI);
    
    
    distance[currentpath] = 0.0;
    
    lossfac[currentpath]  = 1.0;
    
    for (j = 0 ; j < num_pts - 1 ; j++)
    {
        jd         = j*5;
        
        P1P2x      = path[5 + jd] - path[0 + jd];
        
        P1P2y      = path[6 + jd] - path[1 + jd];
        
        P1P2z      = path[7 + jd] - path[2 + jd];
        
        
        
        jd        += 5;
        
        flagP2     = (int)path[4 + jd];
        
        ind        = (int)path[3 + jd];
        
        ind1       = ind*22;
        
        
        uy         = planes[13 + ind1];
        
        dj         = sqrt(P1P2x*P1P2x + P1P2y*P1P2y + P1P2z*P1P2z);
        
        
        
        loss_jreal = 1.0;
        
        loss_jimag = 0.0;
        
        
        if( (flagP2 == 1))
        {
            
            ind2    = ind*6;
            
            epsreal = material[4 + ind2];
            
            epsimag = -60.0*lambda*material[5 + ind2];
            
            tmp     = 1.0/dj;
            
            Vd      = planes[12 + ind1]*P1P2x*tmp + uy*P1P2y*tmp + planes[14 + ind1]*P1P2z*tmp;
            
            vs      = fabs(Vd);
            
            vs2     = vs*vs;
            
            x1      = epsreal - (1.0 - vs2);
            
            y1      = epsimag;
            
            r1      = sqrt(x1*x1 + y1*y1);
            
            x2      = sqrt((r1 + x1)*0.5);
            
            y2      = y1/(2.0*x2);
            
            
            if(fabs(uy) > 0.5)
                
            {
                
                x3         = epsreal*vs - x2;
                
                y3         = epsimag*vs - y2;
                
                
                x4         = epsreal*vs + x2;
                
                y4         = epsimag*vs + y2;
                
                r2         = 1.0/(x4*x4 + y4*y4);
                
                loss_jreal = (x3*x4 + y3*y4)*r2;
                
                loss_jimag = (y3*x4 - x3*y4)*r2;
                
                
            }
            
            else
            {
                
                x3         = vs - x2;
                
                y3         = -y2;
                
                x4         = vs + x2;
                
                y4         = y2;
                
                r2         = 1.0/(x4*x4 + y4*y4);
                
                loss_jreal = (x3*x4 + y3*y4)*r2;
                
                loss_jimag = (y3*x4 - x3*y4)*r2;
                
            }
            
        }
        
        

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -