Интересная идея. Для построения карты можно считать каждый RFID-тег точкой, собирать RSSI с нескольких считывателей и использовать трилатерацию, чтобы определить координаты. Так как RSSI довольно шумный, можно использовать взвешенную аппроксимацию методом наименьших квадратов или фильтр Калмана, чтобы сгладить оценки. Храни карту в простом двумерном массиве и обновляй ее при получении новых данных. Если пространство трехмерное, просто добавь слой высоты для каждого этажа. Сначала нужно откалибровать зависимость RSSI от расстояния – простая линейная регрессия на нескольких тестовых точках хорошо подойдет. Если нужны точные формулы или помощь в настройке фильтра, дай знать.

``` // Простой 2D фильтр Калмана для оценки положения // Предполагается, что у вас есть шаг обновления, который дает вам измерение (x_meas, y_meas) // и вы вызываете predict() через регулярные интервалы class Kalman2D { public: Kalman2D(float dt, float processNoise, float measurementNoise) { dt_ = dt; // Вектор состояния [x, y, vx, vy] A_ = {{1, 0, dt_, 0}, {0, 1, 0, dt_}, {0, 0, 1, 0}, {0, 0, 0, 1}}; H_ = {{1, 0, 0, 0}, {0, 1, 0, 0}}; Q_ = {{processNoise, 0, 0, 0}, {0, processNoise, 0, 0}, {0, 0, processNoise, 0}, {0, 0, 0, processNoise}}; R_ = {{measurementNoise, 0}, {0, measurementNoise}}; P_ = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; x_ = {0, 0, 0, 0}; } void predict() { // x = A*x x_ = multiply(A_, x_); // P = A*P*A^T + Q P_ = add(multiply(A_, multiply(P_, transpose(A_))), Q_); } void update(float measX, float measY) { // y = z - H*x float y[2] = {measX - dot(H_[0], x_), measY - dot(H_[1], x_)}; // S = H*P*H^T + R float S[2][2] = add(multiply(H_, multiply(P_, transpose(H_))), R_); // K = P*H^T*S^-1 float K[4][2]; float invS[2][2] = inverse2x2(S); for (int i=0;i<4;i++){ for (int j=0;j<2;j++){ K[i][j] = 0; for (int k=0;k<4;k++) K[i][j] += P_[i][k]*transpose(H_)[k][j]; K[i][j] *= invS[j][j]; } } // x = x + K*y for (int i=0;i<4;i++) { x_[i] += K[i][0]*y[0] + K[i][1]*y[1]; } // P = (I - K*H)*P float KH[4][4] = multiply(K, H_); float I[4][4] = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}}; float IminusKH[4][4]; for (int i=0;i<4;i++) for (int j=0;j<4;j++) IminusKH[i][j] = I[i][j] - KH[i][j]; P_ = multiply(IminusKH, P_); } float getX() { return x_[0]; } float getY() { return x_[1]; } private: float dt_; float A_[4][4], H_[2][4], Q_[4][4], R_[2][2], P_[4][4]; float x_[4]; // вспомогательные математические функции float dot(float a[4], float b[4]) { return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]+a[3]*b[3]; } float dot(float a[2], float b[4]) { return a[0]*b[0]+a[1]*b[1]; } float dot(float a[4], float b[2]) { return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]+a[3]*b[3]; } float* multiply(float a[4][4], float b[4]) { static float res[4]; for(int i=0;i<4;i++){res[i]=0;for(int k=0;k<4;k++)res[i]+=a[i][k]*b[k];} return res; } float* multiply(float a[4][4], float b[4][4]) { static float res[4][4]; for(int i=0;i<4;i++)for(int j=0;j<4;j++){res[i][j]=0;for(int k=0;k<4;k++)res[i][j]+=a[i][k]*b[k][j];} return (float*)res; } float* multiply(float a[2][4], float b[4][4]) { static float res[2][4]; for(int i=0;i<2;i++)for(int j=0;j<4;j++){res[i][j]=0;for(int k=0;k<4;k++)res[i][j]+=a[i][k]*b[k][j];} return (float*)res; } float* multiply(float a[4][4], float b[4][2]) { static float res[4][2]; for(int i=0;i<4;i++)for(int j=0;j<2;j++){res[i][j]=0;for(int k=0;k<4;k++)res[i][j]+=a[i][k]*b[k][j];} return (float*)res; } float* multiply(float a[4][4], float b[4]) { static float res[4]; for(int i=0;i<4;i++){res[i]=0;for(int k=0;k<4;k++)res[i]+=a[i][k]*b[k];} return res; } float* multiply(float a[2][4], float b[4]) { static float res[2]; for(int i=0;i<2;i++){res[i]=0;for(int k=0;k<4;k++)res[i]+=a[i][k]*b[k];} return res; } float* transpose(float a[4][4]) { static float res[4][4]; for(int i=0;i<4;i++)for(int j=0;j<4;j++)res[j][i]=a[i][j]; return (float*)res; } float* transpose(float a[2][4]) { static float res[4][2]; for(int i=0;i<4;i++)for(int j=0;j<2;j++)res[i][j]=a[j][i]; return (float*)res; } float* transpose(float a[4][2]) { static float res[2][4]; for(int i=0;i<2;i++)for(int j=0;j<4;j++)res[i][j]=a[j][i]; return (float*)res; } float* add(float a[4][4], float b[4][4]) { static float res[4][4]; for(int i=0;i<4;i++)for(int j=0;j<4;j++)res[i][j]=a[i][j]+b[i][j]; return (float*)res; } float* add(float a[2][2], float b[2][2]) { static float res[2][2]; for(int i=0;i<2;i++)for(int j=0;j<2;j++)res[i][j]=a[i][j]+b[i][j]; return (float*)res; } float* inverse2x2(float a[2][2]) { static float res[2][2]; float det = a[0][0]*a[1][1]-a[0][1]*a[1][0]; res[0][0]=a[1][1]/det; res[0][1]=-a[0][1]/det; res[1][0]=-a[1][0]/det; res[1][1]=a[0][0]/det; return (float*)res; } } ```