Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Wls update (#2484)

Browse Source 
* FIX: initialization of WLS residual

* Add a test for an overdetermined WLS problem
This commit is contained in:
Ewoud Smeur
2020-01-29 01:21:40 +01:00
committed by GitHub
parent a27212a6d1
commit d93adeb2e8
2 changed files with 131 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
+1 -1
View File
@@ -170,7 +170,7 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
for (int i = n_v; i < n_c; i++) {
memset(A[i], 0, n_u * sizeof(float));
A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
b[i] = up ? (Wu ? Wu[i] * up[i] : up[i]) : 0;
b[i] = up ? (Wu ? Wu[i-n_v] * up[i-n_v] : up[i-n_v]) : 0;
d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
}
sw/airborne/test/test_alloc.c
+130 -9
View File
@@ -1,19 +1,54 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file test_alloc.c
*
* Test routine for WLS (weighted least squares) control allocation
*
* Comparing a precomputed solution from Matlab with the one coming
* from our PPRZ implementation.
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_double.h"
#include "math/pprz_algebra_int.h"
#include "pprz_algebra_print.h"
#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
#define INDI_OUTPUTS 4
#define INDI_NUM_ACT 4
void test_overdetermined(void);
void calc_nu_out(float** Bwls, float* du, float* nu_out);
int main(int argc, char **argv)
{
#define INDI_NUM_ACT 6
test_overdetermined();
/*#define INDI_NUM_ACT 4*/
/*test_four_by_four();*/
}
/*
* function to test wls with 4x4 (outputs x inputs) system
*/
void test_four_by_four(void)
{
float u_min[INDI_NUM_ACT] = { -107, -19093, 0, -95, };
float u_max[INDI_NUM_ACT] = {19093, 107, 4600, 4505, };
@@ -43,5 +78,91 @@ int main(int argc, char **argv)
// WLS Control Allocator
int num_iter =
wls_alloc(indi_du, indi_v, u_min, u_max, Bwls, INDI_NUM_ACT, INDI_OUTPUTS, 0, 0, Wv, 0, 0, 10000, 10);
printf("finished in %d iterations\n", num_iter);
printf("du = %f, %f, %f, %f\n", indi_du[0], indi_du[1], indi_du[2], indi_du[3]);
}
/*
* function to test wls for an overdetermined 4x6 (outputs x inputs) system
*/
void test_overdetermined(void)
{
float u_min[INDI_NUM_ACT] = {0};
float u_max[INDI_NUM_ACT] = {0};
float du_min[INDI_NUM_ACT] = {0};
float du_max[INDI_NUM_ACT] = {0};
float u_p[INDI_NUM_ACT] = {0};
float u_c[INDI_NUM_ACT] = {4614, 4210, 4210, 4614, 4210, 4210};
printf("lower and upper bounds for du:\n");
uint8_t k;
for(k=0; k<INDI_NUM_ACT; k++) {
u_max[k] = 9600 - u_min[k];
du_min[k] = u_min[k] - u_c[k];
du_max[k] = u_max[k] - u_c[k];
u_p[k] = du_min[k];
printf("%f ", du_min[k]);
printf("%f \n", du_max[k]);
}
printf("\n");
float g1g2[INDI_OUTPUTS][INDI_NUM_ACT] = {
{ 0.0, -0.015, 0.015, 0.0, -0.015, 0.015 },
{ 0.015, -0.010, -0.010, 0.015, -0.010, -0.010 },
{ 0.103, 0.103, 0.103, -0.103, -0.103, -0.103 },
{-0.0009, -0.0009, -0.0009, -0.0009, -0.0009, -0.0009 }
};
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
static float Wv[INDI_OUTPUTS] = {100, 100, 1, 10};
// The control objective in array format
float indi_v[INDI_OUTPUTS] = {240, -240.5658, 600.0, 1.8532};
float indi_du[INDI_NUM_ACT];
// Initialize the array of pointers to the rows of g1g2
float *Bwls[INDI_OUTPUTS];
uint8_t i;
for (i = 0; i < INDI_OUTPUTS; i++) {
Bwls[i] = g1g2[i];
}
// WLS Control Allocator
int num_iter =
wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, INDI_NUM_ACT, INDI_OUTPUTS, 0, 0, Wv, 0, u_p, 0, 10);
printf("finished in %d iterations\n", num_iter);
float nu_out[4] = {0.0f};
calc_nu_out(Bwls, indi_du, nu_out);
printf("du = %f, %f, %f, %f, %f, %f\n", indi_du[0], indi_du[1], indi_du[2], indi_du[3], indi_du[4], indi_du[5]);
// Precomputed solution' in Matlab for this problem using lsqlin:
printf("du (matlab_lsqlin) = %f, %f, %f, %f, %f, %f\n", -4614.0, 426.064612091305, 5390.0, -4614.0, -4210.0, 5390.0);
printf("u = %f, %f, %f, %f, %f, %f\n", indi_du[0]+u_c[0], indi_du[1]+u_c[1], indi_du[2]+u_c[2], indi_du[3]+u_c[3], indi_du[4]+u_c[4], indi_du[5]+u_c[5]);
printf("nu_in = %f, %f, %f, %f\n", indi_v[0], indi_v[1], indi_v[2], indi_v[3]);
printf("nu_out = %f, %f, %f, %f\n", nu_out[0], nu_out[1], nu_out[2], nu_out[3]);
}
/*
* Calculate the achieved control objective for some calculated control input
*/
void calc_nu_out(float** Bwls, float* du, float* nu_out) {
for(int i=0; i<4; i++) {
nu_out[i] = 0;
for(int j=0; j<INDI_NUM_ACT; j++) {
nu_out[i] += Bwls[i][j] * du[j];
}
}
}