total_power3.c

该程序用粒子滤波的方法跟踪一个机器人并根据WiFi信号修改位置信息。

                                                        
                                                        rpath3[5 + kd]  = Phitjk[0];
                                                        
                                                        rpath3[6 + kd]  = Phitjk[1];
                                                        
                                                        rpath3[7 + kd]  = Phitjk[2];
                                                        
                                                        rpath3[8 + kd]  = j;
                                                        
                                                        rpath3[9 + kd]  = 1.0;
                                                        
                                                        rpath3[10 + kd] = Phitk[0];
                                                        
                                                        rpath3[11 + kd] = Phitk[1];
                                                        
                                                        rpath3[12 + kd] = Phitk[2];
                                                        
                                                        rpath3[13 + kd] = k;
                                                        
                                                        rpath3[14 + kd] = 1.0;
                                                        
                                                    }
                                                    
                                                }
                                                
                                            }
                                            
                                        }
                                        
                                    }
                                }
                            }
                            
                        }
                        
                    }
                }
                
            }
            
            npath                  = npath1 + npath2 + npath3 + 1;
                        
            distance               = mxMalloc(npath*sizeof(double));
            
            lossfac                = mxMalloc(npath*sizeof(double));
                        
            
            // Direct path (0 reflexion)
            
            
            Thits0                 = fct_caltpts(TX , RX, planes , nplanes , material , &tflag0 , Phit_nt);
                        
            nb_pts                 = (2 + tflag0);
            
            path0                  = mxMalloc(nb_pts*5*sizeof(double));
            
            
            path0[0]               = TX[0];
            
            path0[1]               = TX[1];
            
            path0[2]               = TX[2];
            
            path0[3]               = 0.0;
            
            path0[4]               = 0.0;
            
            
            
            indice                  = (tflag0 + 1)*5;
            
            
            for(i = 5 ; i < indice; i++)
                
            {
                
                path0[i] = Thits0[i - 5];
                
            }
            
            
            path0[0 + indice]       = RX[0];
            
            path0[1 + indice]       = RX[1];
            
            path0[2 + indice]       = RX[2];
            
            path0[3 + indice]       = 0.0;
            
            path0[4 + indice]       = 0.0;
            
            
            compute_distloss(path0 , nb_pts , planes , material , fc , flp_scale  , currentpath , distance , lossfac);
            
            
            
            if(Thits0 != NULL)
            {
                
                free(Thits0);
                
                Thits0 = NULL;
                
            }
            
            if(path0 != NULL)
            {
                
                mxFree(path0);
                
                path0 = NULL;
                
            }
            
            
            
            
            
            // Path with 1 reflexion
            
            
            for (i = 0 ; i < npath1 ; i++)
                
            {
                
                currentpath++;
                
                id                     = i*5;
                
                
                Phiti[0]               = rpath1[0 + id];
                
                Phiti[1]               = rpath1[1 + id];
                
                Phiti[2]               = rpath1[2 + id];
                
                
                Thits11                = fct_caltpts(TX , Phiti, planes , nplanes , material , &tflag11 , Phit_nt);
                
                Thits12                = fct_caltpts(Phiti, RX, planes , nplanes , material , &tflag12 , Phit_nt);
                
                
                nb_pts                 = (tflag11+tflag12+3);
                
                path1                  = mxMalloc(nb_pts*5*sizeof(double));
                
                
                path1[0]               = TX[0];
                
                path1[1]               = TX[1];
                
                path1[2]               = TX[2];
                
                path1[3]               = 0.0;
                
                path1[4]               = 0.0;
                
                
                indice                  = (tflag11+1)*5;
                
                
                for(j = 5 ; j < indice ; j++)
                    
                {
                    
                    path1[j] = Thits11[j - 5];
                    
                }
                
                
                
                path1[0 + indice]       = Phiti[0];
                
                path1[1 + indice]       = Phiti[1];
                
                path1[2 + indice]       = Phiti[2];
                
                path1[3 + indice]       = rpath1[3 + id];
                
                path1[4 + indice]       = rpath1[4 + id];
                
                
                
                ind                     = (tflag11+2)*5;
                
                indice                  = (tflag11+tflag12+2)*5;
                
                
                for(j = ind  ; j < indice ; j++)
                    
                {
                    
                    path1[j] = Thits12[j - ind];
                    
                }
                
                
                path1[0 + indice]       = RX[0];
                
                path1[1 + indice]       = RX[1];
                
                path1[2 + indice]       = RX[2];
                
                path1[3 + indice]       = 0.0;
                
                path1[4 + indice]       = 0.0;
                
                
                compute_distloss(path1 , nb_pts , planes , material , fc , flp_scale , currentpath , distance , lossfac);
                
                
                if(Thits11 !=NULL)
                {
                    free(Thits11);
                    
                    Thits11 = NULL;
                }
                
                if(Thits12 !=NULL)
                {
                    free(Thits12);
                    
                    Thits12 = NULL;
                    
                }
                
                if(path1 !=NULL)
                {
                    
                    mxFree(path1);
                    
                    path1 = NULL;
                    
                }
                
            }
            
            
            // Path with 2 reflexion
            
            
            for (i = 0 ; i < npath2 ; i++)
                
            {
                
                currentpath++;
                
                id                     = i*10;
                
                
                Phitij[0]              = rpath2[0 + id];
                
                Phitij[1]              = rpath2[1 + id];
                
                Phitij[2]              = rpath2[2 + id];
                
                
                Phitj[0]               = rpath2[5 + id];
                
                Phitj[1]               = rpath2[6 + id];
                
                Phitj[2]               = rpath2[7 + id];
                
                
                Thits21                = fct_caltpts(TX, Phitij, planes , nplanes , material , &tflag21 , Phit_nt);
                
                Thits22                = fct_caltpts(Phitij, Phitj, planes , nplanes , material , &tflag22 , Phit_nt);
                
                Thits23                = fct_caltpts(Phitj, RX, planes , nplanes , material , &tflag23 , Phit_nt);
                
                
                nb_pts                 = (tflag21+tflag22+tflag23+4);
                
                path2                  = mxMalloc(nb_pts*5*sizeof(double));
                
                
                
                
                
                path2[0]               = TX[0];
                
                path2[1]               = TX[1];
                
                path2[2]               = TX[2];
                
                path2[3]               = 0.0;
                
                path2[4]               = 0.0;
                
                indice                 = (tflag21+1)*5;
                
                
                
                for(j = 5 ; j < indice  ; j++)
                    
                {
                    
                    path2[j]            = Thits21[j - 5];
                    
                }
                
                
                
                path2[0 + indice]       = Phitij[0];
                
                path2[1 + indice]       = Phitij[1];
                
                path2[2 + indice]       = Phitij[2];
                
                path2[3 + indice]       = rpath2[3 + id];
                
                path2[4 + indice]       = rpath2[4 + id];
                
                
                
                ind                     = (tflag21+2)*5;
                
                indice                  = (tflag21+tflag22 + 2)*5;
                
                
                for(j = ind ; j < indice ; j++)
                    
                {
                    
                    path2[j] = Thits22[j - ind];
                    
                }
                
                
                
                path2[0 + indice]       = Phitj[0];
                
                path2[1 + indice]       = Phitj[1];
                
                path2[2 + indice]       = Phitj[2];
                
                path2[3 + indice]       = rpath2[8 + id];
                
                path2[4 + indice]       = rpath2[9 + id];
                
                
                
                ind                     = (tflag21+tflag22+3)*5;
                
                indice                  = (tflag21+tflag22+tflag23+3)*5;
                
                
                for(j = ind ; j < indice ; j++)
                    
                {
                    
                    path2[j] = Thits23[j - ind];
                    
                }
                
                
                
                path2[0 + indice]       = RX[0];
                
                path2[1 + indice]       = RX[1];
                
                path2[2 + indice]       = RX[2];
                
                path2[3 + indice]       = 0.0;
                
                path2[4 + indice]       = 0.0;
                
                
                compute_distloss(path2 , nb_pts , planes , material , fc , flp_scale  , currentpath , distance , lossfac);
                
                
                if(Thits21 != NULL)
                {
                    free(Thits21);
                    
                    Thits21 = NULL;
                }
                
                if(Thits22 != NULL)
                {
                    free(Thits22);
                    
                    Thits22 = NULL;
                    
                }
                
                if(Thits23 != NULL)
                {
                    free(Thits23);
                    
                    Thits23 = NULL;
                    
                }
                
                
                if(path2 != NULL)
                {
                    
                    mxFree(path2);
                    
                    path2 = NULL;
                    
                }
                
                
            }
            
            
            
            // Path with 3 reflexions
            
            
            for (i = 0 ; i < npath3 ; i++)
                
            {
                
                currentpath++;
                
                id                     = i*15;
                
                
                Phitijk[0]             = rpath3[0 + id];
                
                Phitijk[1]             = rpath3[1 + id];
                
                Phitijk[2]             = rpath3[2 + id];
                
                
                Phitjk[0]              = rpath3[5 + id];
                
                Phitjk[1]              = rpath3[6 + id];
                
                Phitjk[2]              = rpath3[7 + id];
                
                
                Phitk[0]               = rpath3[10 + id];
                
                Phitk[1]               = rpath3[11 + id];
                
                Phitk[2]               = rpath3[12 + id];
                
                
                
                Thits31                = fct_caltpts(TX , Phitijk, planes , nplanes , material , &tflag31 , Phit_nt);